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