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 static edu.wpi.first.units.Units.*;
010
011import com.ctre.phoenix6.StatusCode;
012import com.ctre.phoenix6.hardware.core.CoreCANcoder;
013import com.ctre.phoenix6.hardware.CANcoder;
014import com.ctre.phoenix6.jni.PlatformJNI;
015import com.ctre.phoenix6.signals.MagnetHealthValue;
016
017import edu.wpi.first.units.measure.*;
018
019/**
020 * Class to control the state of a simulated {@link CANcoder}.
021 */
022public class CANcoderSimState {
023        private static final DeviceType kDevType = DeviceType.P6_CANcoderType;
024
025        private final int _id;
026
027        /**
028         * The orientation of the CANcoder relative to the robot chassis.
029         * <p>
030         * This value should not be changed based on the CANcoder invert.
031         * Rather, this value should be changed when the mechanical linkage
032         * between the CANcoder and the robot changes.
033         */
034        public ChassisReference Orientation;
035
036        /**
037         * Creates an object to control the state of the given {@link CANcoder}.
038         * <p>
039         * This constructor defaults to a counter-clockwise positive orientation
040         * relative to the robot chassis.
041         * <p>
042         * Note the recommended method of accessing simulation features is to use
043         * {@link CANcoder#getSimState}
044         *
045         * @param device Device to which this simulation state is attached
046         */
047        public CANcoderSimState(CoreCANcoder device) {
048                this(device, ChassisReference.CounterClockwise_Positive);
049        }
050        /**
051         * Creates an object to control the state of the given {@link CANcoder}.
052         * <p>
053         * Note the recommended method of accessing simulation features is to use
054         * {@link CANcoder#getSimState}
055         *
056         * @param device Device to which this simulation state is attached
057         * @param orientation Orientation of the device relative to the robot chassis
058         */
059        public CANcoderSimState(CoreCANcoder device, ChassisReference orientation) {
060                _id = device.getDeviceID();
061                Orientation = orientation;
062        }
063
064        /**
065         * Sets the simulated supply voltage of the CANcoder.
066         * <p>
067         * The minimum allowed supply voltage is 4 V - values below this
068         * will be promoted to 4 V.
069         *
070         * @param volts The supply voltage in Volts
071         * @return Status code
072         */
073        public StatusCode setSupplyVoltage(double volts) {
074                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "SupplyVoltage", volts));
075        }
076        /**
077         * Sets the simulated supply voltage of the CANcoder.
078         * <p>
079         * The minimum allowed supply voltage is 4 V - values below this
080         * will be promoted to 4 V.
081         *
082         * @param voltage The supply voltage
083         * @return Status code
084         */
085        public StatusCode setSupplyVoltage(Voltage voltage) {
086                return setSupplyVoltage(voltage.in(Volts));
087        }
088
089        /**
090         * Sets the simulated raw position of the CANcoder.
091         * <p>
092         * Inputs to this function over time should be continuous, as user calls of {@link CANcoder#setPosition} will be accounted for in the callee.
093         * <p>
094         * The CANcoder integrates this to calculate the true reported position.
095         * <p>
096         * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
097         * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
098         * Changes to {@code rawPositionInput} will be integrated into the emulated position.
099         * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
100         *
101         * @param rotations The raw position in rotations
102         * @return Status code
103         */
104        public StatusCode setRawPosition(double rotations) {
105                if (Orientation == ChassisReference.Clockwise_Positive) {
106                        rotations = -rotations;
107                }
108                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawPosition", rotations));
109        }
110        /**
111         * Sets the simulated raw position of the CANcoder.
112         * <p>
113         * Inputs to this function over time should be continuous, as user calls of {@link CANcoder#setPosition} will be accounted for in the callee.
114         * <p>
115         * The CANcoder integrates this to calculate the true reported position.
116         * <p>
117         * When using the WPI Sim GUI, you will notice a readonly {@code position} and settable {@code rawPositionInput}.
118         * The readonly signal is the emulated position which will match self-test in Tuner and the hardware API.
119         * Changes to {@code rawPositionInput} will be integrated into the emulated position.
120         * This way a simulator can modify the position without overriding hardware API calls for home-ing the sensor.
121         *
122         * @param position The raw position
123         * @return Status code
124         */
125        public StatusCode setRawPosition(Angle position) {
126                return setRawPosition(position.in(Rotations));
127        }
128
129        /**
130         * Adds to the simulated position of the CANcoder.
131         *
132         * @param dRotations The change in position in rotations
133         * @return Status code
134         */
135        public StatusCode addPosition(double dRotations) {
136                if (Orientation == ChassisReference.Clockwise_Positive) {
137                        dRotations = -dRotations;
138                }
139                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AddPosition", dRotations));
140        }
141        /**
142         * Adds to the simulated position of the CANcoder.
143         *
144         * @param dPosition The change in position
145         * @return Status code
146         */
147        public StatusCode addPosition(Angle dPosition) {
148                return addPosition(dPosition.in(Rotations));
149        }
150
151        /**
152         * Sets the simulated velocity of the CANcoder.
153         *
154         * @param rps The new velocity in rotations per second
155         * @return Status code
156         */
157        public StatusCode setVelocity(double rps) {
158                if (Orientation == ChassisReference.Clockwise_Positive) {
159                        rps = -rps;
160                }
161                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "Velocity", rps));
162        }
163        /**
164         * Sets the simulated velocity of the CANcoder.
165         *
166         * @param velocity The new velocity
167         * @return Status code
168         */
169        public StatusCode setVelocity(AngularVelocity velocity) {
170                return setVelocity(velocity.in(RotationsPerSecond));
171        }
172
173        /**
174         * Sets the simulated magnet health of the CANcoder.
175         *
176         * @param value The magnet health to simulate. This directly correlates to the 
177         *              red/green/orange state of the simulated LED.
178         * @return Status code
179         */
180        public StatusCode setMagnetHealth(MagnetHealthValue value) {
181                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "MagnetHealth", value.value));
182        }
183}