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.CorePigeon2;
013import com.ctre.phoenix6.hardware.Pigeon2;
014import com.ctre.phoenix6.jni.PlatformJNI;
015
016import edu.wpi.first.units.measure.*;
017
018/**
019 * Class to control the state of a simulated {@link Pigeon2}.
020 */
021public class Pigeon2SimState {
022        private static final DeviceType kDevType = DeviceType.P6_Pigeon2Type;
023
024        private final int _id;
025
026        /**
027         * Creates an object to control the state of the given {@link Pigeon2}.
028         * <p>
029         * Note the recommended method of accessing simulation features is to use
030         * {@link Pigeon2#getSimState}
031         * 
032         * @param device Device to which this simulation state is attached
033         */
034        public Pigeon2SimState(CorePigeon2 device) {
035                _id = device.getDeviceID();
036        }
037
038        /**
039         * Sets the simulated supply voltage of the Pigeon2.
040         * <p>
041         * The minimum allowed supply voltage is 4 V - values below this
042         * will be promoted to 4 V.
043         *
044         * @param volts The supply voltage in Volts
045         * @return Status code
046         */
047        public StatusCode setSupplyVoltage(double volts) {
048                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "SupplyVoltage", volts));
049        }
050        /**
051         * Sets the simulated supply voltage of the Pigeon2.
052         * <p>
053         * The minimum allowed supply voltage is 4 V - values below this
054         * will be promoted to 4 V.
055         *
056         * @param voltage The supply voltage
057         * @return Status code
058         */
059        public StatusCode setSupplyVoltage(Voltage voltage) {
060                return setSupplyVoltage(voltage.in(Volts));
061        }
062
063        /**
064         * Sets the simulated raw yaw of the Pigeon2.
065         * <p>
066         * Inputs to this function over time should be continuous, as user calls of {@link Pigeon2#setYaw} will be accounted for in the callee.
067         * <p>
068         * The Pigeon2 integrates this to calculate the true reported yaw.
069         * <p>
070         * When using the WPI Sim GUI, you will notice a readonly {@code yaw} and settable {@code rawYawInput}.
071         * The readonly signal is the emulated yaw which will match self-test in Tuner and the hardware API.
072         * Changes to {@code rawYawInput} will be integrated into the emulated yaw.
073         * This way a simulator can modify the yaw without overriding hardware API calls for home-ing the sensor.
074         *
075         * @param deg The yaw in degrees
076         * @return Status code
077         */
078        public StatusCode setRawYaw(double deg) {
079                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "RawYaw", deg));
080        }
081        /**
082         * Sets the simulated raw yaw of the Pigeon2.
083         * <p>
084         * Inputs to this function over time should be continuous, as user calls of {@link Pigeon2#setYaw} will be accounted for in the callee.
085         * <p>
086         * The Pigeon2 integrates this to calculate the true reported yaw.
087         * <p>
088         * When using the WPI Sim GUI, you will notice a readonly {@code yaw} and settable {@code rawYawInput}.
089         * The readonly signal is the emulated yaw which will match self-test in Tuner and the hardware API.
090         * Changes to {@code rawYawInput} will be integrated into the emulated yaw.
091         * This way a simulator can modify the yaw without overriding hardware API calls for home-ing the sensor.
092         *
093         * @param yaw The yaw
094         * @return Status code
095         */
096        public StatusCode setRawYaw(Angle yaw) {
097                return setRawYaw(yaw.in(Degrees));
098        }
099
100        /**
101         * Adds to the simulated yaw of the Pigeon2.
102         *
103         * @param dDeg The change in yaw in degrees
104         * @return Status code
105         */
106        public StatusCode addYaw(double dDeg) {
107                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AddYaw", dDeg));
108        }
109        /**
110         * Adds to the simulated yaw of the Pigeon2.
111         *
112         * @param dYaw The change in yaw
113         * @return Status code
114         */
115        public StatusCode addYaw(Angle dYaw) {
116                return addYaw(dYaw.in(Degrees));
117        }
118
119        /**
120         * Sets the simulated pitch of the Pigeon2.
121         *
122         * @param deg The pitch in degrees
123         * @return Status code
124         */
125        public StatusCode setPitch(double deg) {
126                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "Pitch", deg));
127        }
128        /**
129         * Sets the simulated pitch of the Pigeon2.
130         *
131         * @param pitch The pitch
132         * @return Status code
133         */
134        public StatusCode setPitch(Angle pitch) {
135                return setPitch(pitch.in(Degrees));
136        }
137
138        /**
139         * Sets the simulated roll of the Pigeon2.
140         *
141         * @param deg The roll in degrees
142         * @return Status code
143         */
144        public StatusCode setRoll(double deg) {
145                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "Roll", deg));
146        }
147        /**
148         * Sets the simulated roll of the Pigeon2.
149         *
150         * @param roll The roll
151         * @return Status code
152         */
153        public StatusCode setRoll(Angle roll) {
154                return setRoll(roll.in(Degrees));
155        }
156
157        /**
158         * Sets the simulated angular velocity X component of the Pigeon2.
159         *
160         * @param dps The X component of the angular velocity in degrees per second
161         * @return Status code
162         */
163        public StatusCode setAngularVelocityX(double dps) {
164                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AngularVelocityX", dps));
165        }
166        /**
167         * Sets the simulated angular velocity X component of the Pigeon2.
168         *
169         * @param angularVel The X component of the angular velocity
170         * @return Status code
171         */
172        public StatusCode setAngularVelocityX(AngularVelocity angularVel) {
173                return setAngularVelocityX(angularVel.in(DegreesPerSecond));
174        }
175
176        /**
177         * Sets the simulated angular velocity Y component of the Pigeon2.
178         *
179         * @param dps The Y component of the angular velocity in degrees per second
180         * @return Status code
181         */
182        public StatusCode setAngularVelocityY(double dps) {
183                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AngularVelocityY", dps));
184        }
185        /**
186         * Sets the simulated angular velocity Y component of the Pigeon2.
187         *
188         * @param angularVel The Y component of the angular velocity
189         * @return Status code
190         */
191        public StatusCode setAngularVelocityY(AngularVelocity angularVel) {
192                return setAngularVelocityY(angularVel.in(DegreesPerSecond));
193        }
194
195        /**
196         * Sets the simulated angular velocity Z component of the Pigeon2.
197         *
198         * @param dps The Z component of the angular velocity in degrees per second
199         * @return Status code
200         */
201        public StatusCode setAngularVelocityZ(double dps) {
202                return StatusCode.valueOf(PlatformJNI.JNI_SimSetPhysicsInput(kDevType.value, _id, "AngularVelocityZ", dps));
203        }
204        /**
205         * Sets the simulated angular velocity Z component of the Pigeon2.
206         *
207         * @param angularVel The Z component of the angular velocity
208         * @return Status code
209         */
210        public StatusCode setAngularVelocityZ(AngularVelocity angularVel) {
211                return setAngularVelocityZ(angularVel.in(DegreesPerSecond));
212        }
213}