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.swerve.jni;
008
009import java.util.function.IntSupplier;
010
011import com.ctre.phoenix6.jni.CtreJniWrapper;
012
013public class SwerveJNI extends CtreJniWrapper implements Cloneable {
014    public static class ModuleState {
015        public double speed = 0;
016        public double angle = 0;
017    }
018    public static class ModulePosition {
019        public double distance = 0;
020        public double angle = 0;
021    }
022    public static class DriveState implements Cloneable {
023        /** The current pose of the robot */
024        public double PoseX;
025        public double PoseY;
026        public double PoseTheta;
027        /** The current velocity of the robot */
028        public double SpeedsVx;
029        public double SpeedsVy;
030        public double SpeedsOmega;
031        /** The current module states */
032        public ModuleState[] ModuleStates;
033        /** The target module states */
034        public ModuleState[] ModuleTargets;
035        /** The current module positions */
036        public ModulePosition[] ModulePositions;
037        /** The raw heading of the robot, unaffected by vision updates and odometry resets */
038        public double RawHeading;
039        /** The timestamp of the state capture, in seconds */
040        public double Timestamp;
041        /** The measured odometry update period, in seconds */
042        public double OdometryPeriod;
043        /** Number of successful data acquisitions */
044        public int SuccessfulDaqs;
045        /** Number of failed data acquisitions */
046        public int FailedDaqs;
047
048        @Override
049        public DriveState clone() {
050            final DriveState toReturn = new DriveState();
051
052            toReturn.ModuleStates = new ModuleState[ModuleStates.length];
053            for (int i = 0; i < ModuleStates.length; ++i) {
054                toReturn.ModuleStates[i] = new ModuleState();
055            }
056            toReturn.ModuleTargets = new ModuleState[ModuleTargets.length];
057            for (int i = 0; i < ModuleTargets.length; ++i) {
058                toReturn.ModuleTargets[i] = new ModuleState();
059            }
060            toReturn.ModulePositions = new ModulePosition[ModulePositions.length];
061            for (int i = 0; i < ModulePositions.length; ++i) {
062                toReturn.ModulePositions[i] = new ModulePosition();
063            }
064
065            return toReturn;
066        }
067    }
068    public static class ControlParams {
069        /* Max speed in m/s */
070        public double kMaxSpeedMps;
071        /* Operator forward direction in radians */
072        public double operatorForwardDirection;
073        /* Current chassis speeds of the robot */
074        public double currentChassisSpeedVx;
075        public double currentChassisSpeedVy;
076        public double currentChassisSpeedOmega;
077        /* Current pose of the robot */
078        public double currentPoseX;
079        public double currentPoseY;
080        public double currentPoseTheta;
081        /* Current time */
082        public double timestamp;
083        /* Update rate */
084        public double updatePeriod;
085    }
086    public static class ModuleApplyParams {
087        /* Speed and direction the module should target */
088        public ModuleState state = new ModuleState();
089        /* Robot-relative wheel force feedforward to apply in the X direction */
090        public double wheelForceFeedforwardX = 0.0;
091        /* Robot-relative wheel force feedforward to apply in the Y direction */
092        public double wheelForceFeedforwardY = 0.0;
093        /* The type of control request to use for the drive motor */
094        public int driveRequest = 0;
095        /* The type of control request to use for the steer motor */
096        public int steerRequest = 0;
097        /* The update period of the module request */
098        public double updatePeriod = 0.0;
099        /* Whether to use FOC for Voltage-based control */
100        public boolean enableFOC = true;
101    }
102
103    public DriveState driveState = new DriveState();
104    public ControlParams controlParams = new ControlParams();
105
106    public ModuleApplyParams moduleApplyParams = new ModuleApplyParams();
107    public ModuleState moduleState = new ModuleState();
108    public ModulePosition modulePosition = new ModulePosition();
109
110    @Override
111    public SwerveJNI clone() {
112        SwerveJNI toReturn = new SwerveJNI();
113        toReturn.driveState = driveState.clone();
114        return toReturn;
115    }
116
117    public static native int JNI_CreateDrivetrain(long drivetrainConstants, long moduleConstants, int numModules);
118    public static native int JNI_CreateDrivetrainWithFreq(long drivetrainConstants, double odometryUpdateFreqHz, long moduleConstants, int numModules);
119    public static native int JNI_CreateDrivetrainWithStddev(long drivetrainConstants, double odometryUpdateFreqHz, double[] odometryStandardDeviation,
120            double[] visionStandardDeviation, long moduleConstants, int numModules);
121    public static native void JNI_DestroyDrivetrain(int id);
122
123    public static native boolean JNI_IsOnCANFD(int id);
124    public static native double JNI_GetOdometryFrequency(int id);
125    public static native boolean JNI_IsOdometryValid(int id);
126    public native long JNI_SetControl(int id, IntSupplier request);
127    public static native void JNI_DestroyControl(long handle);
128    public native void JNI_GetState(int id);
129    public native long JNI_RegisterTelemetry(int id, Runnable telemetryFunc);
130    public static native void JNI_DestroyTelemetry(long handle);
131    public static native int JNI_ConfigNeutralMode(int id, int neutralMode);
132    public static native void JNI_TareEverything(int id);
133    public static native void JNI_SeedFieldCentric(int id);
134    public static native void JNI_ResetPose(int id, double xMeters, double yMeters, double rotationRad);
135    public static native void JNI_ResetTranslation(int id, double xMeters, double yMeters);
136    public static native void JNI_ResetRotation(int id, double rotationRad);
137    public static native void JNI_SetOperatorPerspectiveForward(int id, double rotationRad);
138    public static native double JNI_GetOperatorForwardDirection(int id);
139    public static native void JNI_AddVisionMeasurement(int id, double xMeters, double yMeters, double rotationRad, double timestampSeconds);
140    public static native void JNI_AddVisionMeasurementWithStdDev(int id, double xMeters, double yMeters, double rotationRad,
141            double timestampSeconds, double[] visionMeasurementStdDevs);
142    public static native void JNI_SetVisionMeasurementStdDevs(int id, double[] visionMeasurementStdDevs);
143    public static native void JNI_SetStateStdDevs(int id, double[] stateStdDevs);
144    public static native double[] JNI_SamplePoseAt(int id, double timestampSeconds);
145
146    public static native void JNI_Odom_Start(int id);
147    public static native void JNI_Odom_Stop(int id);
148    public static native void JNI_Odom_SetThreadPriority(int id, int priority);
149
150    public native void JNI_Module_Apply(int id, int idx);
151    public native void JNI_Module_GetPosition(int id, int idx, boolean refresh);
152    public native void JNI_Module_GetCachedPosition(int id, int idx);
153    public native void JNI_Module_GetCurrentState(int id, int idx);
154    public native void JNI_Module_GetTargetState(int id, int idx);
155    public static native void JNI_Module_ResetPosition(int id, int idx);
156
157    public static native long JNI_CreateDrivetrainConstants(
158        String canbusName,
159        int pigeon2Id);
160    public static native long JNI_CreateModuleConstantsArr(long numModules);
161    public static native void JNI_SetModuleConstants(
162        long constantsArr,
163        long moduleIdx,
164        int steerMotorId,
165        int driveMotorId,
166        int encoderId,
167        double encoderOffset_tr,
168        double locationX_m,
169        double locationY_m,
170        boolean driveMotorInverted,
171        boolean steerMotorInverted,
172        boolean encoderInverted,
173        double driveMotorGearRatio,
174        double steerMotorGearRatio,
175        double couplingGearRatio,
176        double wheelRadius_m,
177        int steerMotorClosedLoopOutput,
178        int driveMotorClosedLoopOutput,
179        double slipCurrent_A,
180        double speedAt12Volts_mps,
181        int driveMotorType,
182        int steerMotorType,
183        int feedbackSource,
184        double steerInertia_kg_sq_m,
185        double driveInertia_kg_sq_m,
186        double steerFrictionVoltage_V,
187        double driveFrictionVoltage_V);
188    public static native void JNI_DestroyConstants(long constants);
189
190    public static native void JNI_SetControl_Idle(
191        int id);
192    public static native int JNI_Request_Apply_Idle(
193        int id);
194    public static native void JNI_SetControl_SwerveDriveBrake(
195        int id,
196        int driveRequestType,
197        int steerRequestType);
198    public static native int JNI_Request_Apply_SwerveDriveBrake(
199        int id,
200        int driveRequestType,
201        int steerRequestType);
202    public static native void JNI_SetControl_FieldCentric(
203        int id,
204        double velocityX_mps,
205        double velocityY_mps,
206        double rotationalRate_rad_per_s,
207        double deadband_mps,
208        double rotationalDeadband_rad_per_s,
209        double centerOfRotationX,
210        double centerOfRotationY,
211        int driveRequestType,
212        int steerRequestType,
213        boolean desaturateWheelSpeeds,
214        int forwardPerspective);
215    public static native int JNI_Request_Apply_FieldCentric(
216        int id,
217        double velocityX_mps,
218        double velocityY_mps,
219        double rotationalRate_rad_per_s,
220        double deadband_mps,
221        double rotationalDeadband_rad_per_s,
222        double centerOfRotationX,
223        double centerOfRotationY,
224        int driveRequestType,
225        int steerRequestType,
226        boolean desaturateWheelSpeeds,
227        int forwardPerspective);
228    public static native void JNI_SetControl_RobotCentric(
229        int id,
230        double velocityX_mps,
231        double velocityY_mps,
232        double rotationalRate_rad_per_s,
233        double deadband_mps,
234        double rotationalDeadband_rad_per_s,
235        double centerOfRotationX,
236        double centerOfRotationY,
237        int driveRequestType,
238        int steerRequestType,
239        boolean desaturateWheelSpeeds);
240    public static native int JNI_Request_Apply_RobotCentric(
241        int id,
242        double velocityX_mps,
243        double velocityY_mps,
244        double rotationalRate_rad_per_s,
245        double deadband_mps,
246        double rotationalDeadband_rad_per_s,
247        double centerOfRotationX,
248        double centerOfRotationY,
249        int driveRequestType,
250        int steerRequestType,
251        boolean desaturateWheelSpeeds);
252    public static native void JNI_SetControl_PointWheelsAt(
253        int id,
254        double moduleDirection,
255        int driveRequestType,
256        int steerRequestType);
257    public static native int JNI_Request_Apply_PointWheelsAt(
258        int id,
259        double moduleDirection,
260        int driveRequestType,
261        int steerRequestType);
262    public static native void JNI_SetControl_ApplyRobotSpeeds(
263        int id,
264        double speedsVx,
265        double speedsVy,
266        double speedsOmega,
267        double[] wheelForceFeedforwardsX_N,
268        double[] wheelForceFeedforwardsY_N,
269        double centerOfRotationX,
270        double centerOfRotationY,
271        int driveRequestType,
272        int steerRequestType,
273        boolean desaturateWheelSpeeds);
274    public static native int JNI_Request_Apply_ApplyRobotSpeeds(
275        int id,
276        double speedsVx,
277        double speedsVy,
278        double speedsOmega,
279        double[] wheelForceFeedforwardsX_N,
280        double[] wheelForceFeedforwardsY_N,
281        double centerOfRotationX,
282        double centerOfRotationY,
283        int driveRequestType,
284        int steerRequestType,
285        boolean desaturateWheelSpeeds);
286    public static native void JNI_SetControl_ApplyFieldSpeeds(
287        int id,
288        double speedsVx,
289        double speedsVy,
290        double speedsOmega,
291        double[] wheelForceFeedforwardsX_N,
292        double[] wheelForceFeedforwardsY_N,
293        double centerOfRotationX,
294        double centerOfRotationY,
295        int driveRequestType,
296        int steerRequestType,
297        boolean desaturateWheelSpeeds,
298        int forwardPerspective);
299    public static native int JNI_Request_Apply_ApplyFieldSpeeds(
300        int id,
301        double speedsVx,
302        double speedsVy,
303        double speedsOmega,
304        double[] wheelForceFeedforwardsX_N,
305        double[] wheelForceFeedforwardsY_N,
306        double centerOfRotationX,
307        double centerOfRotationY,
308        int driveRequestType,
309        int steerRequestType,
310        boolean desaturateWheelSpeeds,
311        int forwardPerspective);
312}