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.phoenixpro.sim;
008
009import com.ctre.phoenixpro.StatusCode;
010import com.ctre.phoenixpro.hardware.core.CoreTalonFX;
011import com.ctre.phoenixpro.hardware.TalonFX;
012import com.ctre.phoenixpro.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         *
037         * @param device
038         *        Device to which this simulation state is attached
039         */
040        public TalonFXSimState(CoreTalonFX device) {
041                this(device, ChassisReference.CounterClockwise_Positive);
042        }
043        /**
044         * Creates an object to control the state of the given {@link TalonFX}.
045         *
046         * @param device
047         *        Device to which this simulation state is attached
048         * @param orientation
049         *        Orientation of the device relative to the robot chassis
050         */
051        public TalonFXSimState(CoreTalonFX device, ChassisReference orientation) {
052                _id = device.getDeviceID();
053                Orientation = orientation;
054        }
055
056        /**
057         * Gets the last status code generated by a simulation function.
058         * <p>
059         * Not all functions return a status code but can potentially report errors.
060         * This function can be used to retrieve those status codes.
061         *
062         * @return Last status code generated by a simulation function
063         */
064        public StatusCode getLastStatusCode() {
065                return StatusCode.valueOf(PlatformJNI.JNI_SimGetLastError(kDevType.value, _id));
066        }
067        /**
068         * Gets the simulated output voltage of the motor.
069         *
070         * @return Voltage applied to the motor in Volts
071         */
072        public double getMotorVoltage() {
073                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "MotorVoltage");
074                if (Orientation == ChassisReference.Clockwise_Positive) {
075                        value = -value;
076                }
077                return value;
078        }
079        /**
080         * Gets the simulated output torque current of the motor.
081         *
082         * @return Torque current applied to the motor in Amperes
083         */
084        public double getTorqueCurrent() {
085                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "TorqueCurrent");
086                if (Orientation == ChassisReference.Clockwise_Positive) {
087                        value = -value;
088                }
089                return value;
090        }
091        /**
092         * Gets the simulated supply current of the TalonFX.
093         *
094         * @return Supply current of the TalonFX in Amperes
095         */
096        public double getSupplyCurrent() {
097                double value = PlatformJNI.JNI_SimGetPhysicsValue(kDevType.value, _id, "SupplyCurrent");
098                return value;
099        }
100
101        /**
102         * Sets the simulated supply voltage of the TalonFX.
103         * <p>
104         * The minimum allowed supply voltage is 4 V - values below this
105         * will be promoted to 4 V.
106         *
107         * @param volts
108         *        The supply voltage in Volts
109         * @return Status code
110         */
111        public StatusCode setSupplyVoltage(double volts) {
112                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "SupplyVoltage", volts));
113        }
114        /**
115         * Sets the simulated forward limit switch of the TalonFX.
116         *
117         * @param closed
118         *        Whether the limit switch is closed
119         * @return Status code
120         */
121        public StatusCode setForwardLimit(boolean closed) {
122                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "ForwardLimit", closed ? 1 : 0));
123        }
124        /**
125         * Sets the simulated reverse limit switch of the TalonFX.
126         *
127         * @param closed
128         *        Whether the limit switch is closed
129         * @return Status code
130         */
131        public StatusCode setReverseLimit(boolean closed) {
132                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "ReverseLimit", closed ? 1 : 0));
133        }
134        /**
135         * Sets the simulated raw rotor position of the TalonFX.
136         * <p>
137         * Inputs to this function over time should be continuous, as user calls of {@link TalonFX#setRotorPosition} will be accounted for in the callee.
138         * <p>
139         * The TalonFX integrates this to calculate the true reported rotor position.
140         * <p>
141         * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
142         * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
143         * Changes to {@code rawPositionInput} will be integrated into the emulated position.
144         * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
145         *
146         * @param rotations
147         *        The raw position in rotations
148         * @return Status code
149         */
150        public StatusCode setRawRotorPosition(double rotations) {
151                if (Orientation == ChassisReference.Clockwise_Positive) {
152                        rotations = -rotations;
153                }
154                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawRotorPosition", rotations));
155        }
156        /**
157         * Adds to the simulated rotor position of the TalonFX.
158         *
159         * @param dRotations
160         *        The change in position in rotations
161         * @return Status code
162         */
163        public StatusCode addRotorPosition(double dRotations) {
164                if (Orientation == ChassisReference.Clockwise_Positive) {
165                        dRotations = -dRotations;
166                }
167                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AddRotorPosition", dRotations));
168        }
169        /**
170         * Sets the simulated rotor velocity of the TalonFX.
171         *
172         * @param rps
173         *        The new velocity in rotations per second
174         * @return Status code
175         */
176        public StatusCode setRotorVelocity(double rps) {
177                if (Orientation == ChassisReference.Clockwise_Positive) {
178                        rps = -rps;
179                }
180                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RotorVelocity", rps));
181        }
182}