001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.sim;
008
009import com.ctre.phoenix6.StatusCode;
010import com.ctre.phoenix6.hardware.core.CoreTalonFX;
011import com.ctre.phoenix6.hardware.TalonFX;
012import com.ctre.phoenix6.jni.PlatformJNI;
013
014/**
015 * Class to control the state of a simulated {@link TalonFX}.
016 */
017public class TalonFXSimState {
018        private final DeviceType kDevType = DeviceType.PRO_TalonFXType;
019
020        private final int _id;
021
022        /**
023         * The orientation of the TalonFX relative to the robot chassis.
024         * <p>
025         * This value should not be changed based on the TalonFX invert.
026         * Rather, this value should be changed when the mechanical linkage
027         * between the TalonFX and the robot changes.
028         */
029        public ChassisReference Orientation;
030
031        /**
032         * Creates an object to control the state of the given {@link TalonFX}.
033         * <p>
034         * This constructor defaults to a counter-clockwise positive orientation
035         * relative to the robot chassis.
036         * <p>
037         * Note the recommended method of accessing simulation features is to use
038         * {@link TalonFX#getSimState}
039         *
040         * @param device
041         *        Device to which this simulation state is attached
042         */
043        public TalonFXSimState(CoreTalonFX device) {
044                this(device, ChassisReference.CounterClockwise_Positive);
045        }
046        /**
047         * Creates an object to control the state of the given {@link TalonFX}.
048         * <p>
049         * Note the recommended method of accessing simulation features is to use
050         * {@link TalonFX#getSimState}
051         *
052         * @param device
053         *        Device to which this simulation state is attached
054         * @param orientation
055         *        Orientation of the device relative to the robot chassis
056         */
057        public TalonFXSimState(CoreTalonFX device, ChassisReference orientation) {
058                _id = device.getDeviceID();
059                Orientation = orientation;
060        }
061
062        /**
063         * Gets the last status code generated by a simulation function.
064         * <p>
065         * Not all functions return a status code but can potentially report errors.
066         * This function can be used to retrieve those status codes.
067         *
068         * @return Last status code generated by a simulation function
069         */
070        public StatusCode getLastStatusCode() {
071                return StatusCode.valueOf(PlatformJNI.JNI_SimGetLastError(kDevType.value, _id));
072        }
073        /**
074         * Gets the simulated output voltage of the motor.
075         *
076         * @return Voltage applied to the motor in Volts
077         */
078        public double getMotorVoltage() {
079                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "MotorVoltage");
080                if (Orientation == ChassisReference.Clockwise_Positive) {
081                        value = -value;
082                }
083                return value;
084        }
085        /**
086         * Gets the simulated output torque current of the motor.
087         * <p>
088         * Phoenix 6 simulation automatically calculates current.
089         *
090         * @return Torque current applied to the motor in Amperes
091         */
092        public double getTorqueCurrent() {
093                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "TorqueCurrent");
094                if (Orientation == ChassisReference.Clockwise_Positive) {
095                        value = -value;
096                }
097                return value;
098        }
099        /**
100         * Gets the simulated supply current of the TalonFX.
101         * <p>
102         * Phoenix 6 simulation automatically calculates current.
103         *
104         * @return Supply current of the TalonFX in Amperes
105         */
106        public double getSupplyCurrent() {
107                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "SupplyCurrent");
108                return value;
109        }
110
111        /**
112         * Sets the simulated supply voltage of the TalonFX.
113         * <p>
114         * The minimum allowed supply voltage is 4 V - values below this
115         * will be promoted to 4 V.
116         *
117         * @param volts
118         *        The supply voltage in Volts
119         * @return Status code
120         */
121        public StatusCode setSupplyVoltage(double volts) {
122                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "SupplyVoltage", volts));
123        }
124        /**
125         * Sets the simulated forward limit switch of the TalonFX.
126         *
127         * @param closed
128         *        Whether the limit switch is closed
129         * @return Status code
130         */
131        public StatusCode setForwardLimit(boolean closed) {
132                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "ForwardLimit", closed ? 1 : 0));
133        }
134        /**
135         * Sets the simulated reverse limit switch of the TalonFX.
136         *
137         * @param closed
138         *        Whether the limit switch is closed
139         * @return Status code
140         */
141        public StatusCode setReverseLimit(boolean closed) {
142                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "ReverseLimit", closed ? 1 : 0));
143        }
144        /**
145         * Sets the simulated raw rotor position of the TalonFX.
146         * <p>
147         * Inputs to this function over time should be continuous, as user calls of {@link TalonFX#setPosition} will be accounted for in the callee.
148         * <p>
149         * The TalonFX integrates this to calculate the true reported rotor position.
150         * <p>
151         * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
152         * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
153         * Changes to {@code rawPositionInput} will be integrated into the emulated position.
154         * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
155         *
156         * @param rotations
157         *        The raw position in rotations
158         * @return Status code
159         */
160        public StatusCode setRawRotorPosition(double rotations) {
161                if (Orientation == ChassisReference.Clockwise_Positive) {
162                        rotations = -rotations;
163                }
164                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawRotorPosition", rotations));
165        }
166        /**
167         * Adds to the simulated rotor position of the TalonFX.
168         *
169         * @param dRotations
170         *        The change in position in rotations
171         * @return Status code
172         */
173        public StatusCode addRotorPosition(double dRotations) {
174                if (Orientation == ChassisReference.Clockwise_Positive) {
175                        dRotations = -dRotations;
176                }
177                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AddRotorPosition", dRotations));
178        }
179        /**
180         * Sets the simulated rotor velocity of the TalonFX.
181         *
182         * @param rps
183         *        The new velocity in rotations per second
184         * @return Status code
185         */
186        public StatusCode setRotorVelocity(double rps) {
187                if (Orientation == ChassisReference.Clockwise_Positive) {
188                        rps = -rps;
189                }
190                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RotorVelocity", rps));
191        }
192        /**
193         * Sets the simulated rotor acceleration of the TalonFX.
194         *
195         * @param rpss
196         *        The new acceleration in rotations per second²
197         * @return Status code
198         */
199        public StatusCode setRotorAcceleration(double rpss) {
200                if (Orientation == ChassisReference.Clockwise_Positive) {
201                        rpss = -rpss;
202                }
203                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RotorAcceleration", rpss));
204        }
205}