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 int JNI_ConfigNeutralModeWithTimeout(int id, int neutralMode, double timeoutSeconds);
133    public static native void JNI_TareEverything(int id);
134    public static native void JNI_SeedFieldCentric(int id, double rotationRad);
135    public static native void JNI_ResetPose(int id, double xMeters, double yMeters, double rotationRad);
136    public static native void JNI_ResetTranslation(int id, double xMeters, double yMeters);
137    public static native void JNI_ResetRotation(int id, double rotationRad);
138    public static native void JNI_SetOperatorPerspectiveForward(int id, double rotationRad);
139    public static native double JNI_GetOperatorForwardDirection(int id);
140    public static native void JNI_AddVisionMeasurement(int id, double xMeters, double yMeters, double rotationRad, double timestampSeconds);
141    public static native void JNI_AddVisionMeasurementWithStdDev(int id, double xMeters, double yMeters, double rotationRad,
142            double timestampSeconds, double[] visionMeasurementStdDevs);
143    public static native void JNI_SetVisionMeasurementStdDevs(int id, double[] visionMeasurementStdDevs);
144    public static native void JNI_SetStateStdDevs(int id, double[] stateStdDevs);
145    public static native double[] JNI_SamplePoseAt(int id, double timestampSeconds);
146
147    public static native void JNI_Odom_Start(int id);
148    public static native void JNI_Odom_Stop(int id);
149    public static native void JNI_Odom_SetThreadPriority(int id, int priority);
150
151    public native void JNI_Module_Apply(int id, int idx);
152    public native void JNI_Module_GetPosition(int id, int idx, boolean refresh);
153    public native void JNI_Module_GetCachedPosition(int id, int idx);
154    public native void JNI_Module_GetCurrentState(int id, int idx);
155    public native void JNI_Module_GetTargetState(int id, int idx);
156    public static native void JNI_Module_ResetPosition(int id, int idx);
157
158    public static native long JNI_CreateDrivetrainConstants(
159        String canbusName,
160        int pigeon2Id);
161    public static native long JNI_CreateModuleConstantsArr(long numModules);
162    public static native void JNI_SetModuleConstants(
163        long constantsArr,
164        long moduleIdx,
165        int steerMotorId,
166        int driveMotorId,
167        int encoderId,
168        double encoderOffset_tr,
169        double locationX_m,
170        double locationY_m,
171        boolean driveMotorInverted,
172        boolean steerMotorInverted,
173        boolean encoderInverted,
174        double driveMotorGearRatio,
175        double steerMotorGearRatio,
176        double couplingGearRatio,
177        double wheelRadius_m,
178        int steerMotorClosedLoopOutput,
179        int driveMotorClosedLoopOutput,
180        double slipCurrent_A,
181        double speedAt12Volts_mps,
182        int driveMotorType,
183        int steerMotorType,
184        int feedbackSource,
185        double steerInertia_kg_sq_m,
186        double driveInertia_kg_sq_m,
187        double steerFrictionVoltage_V,
188        double driveFrictionVoltage_V);
189    public static native void JNI_DestroyConstants(long constants);
190
191    public static native void JNI_SetControl_Idle(
192        int id);
193    public static native int JNI_Request_Apply_Idle(
194        int id);
195    public static native void JNI_SetControl_SwerveDriveBrake(
196        int id,
197        int driveRequestType,
198        int steerRequestType);
199    public static native int JNI_Request_Apply_SwerveDriveBrake(
200        int id,
201        int driveRequestType,
202        int steerRequestType);
203    public static native void JNI_SetControl_FieldCentric(
204        int id,
205        double velocityX_mps,
206        double velocityY_mps,
207        double rotationalRate_rad_per_s,
208        double deadband_mps,
209        double rotationalDeadband_rad_per_s,
210        double centerOfRotationX,
211        double centerOfRotationY,
212        int driveRequestType,
213        int steerRequestType,
214        boolean desaturateWheelSpeeds,
215        int forwardPerspective);
216    public static native int JNI_Request_Apply_FieldCentric(
217        int id,
218        double velocityX_mps,
219        double velocityY_mps,
220        double rotationalRate_rad_per_s,
221        double deadband_mps,
222        double rotationalDeadband_rad_per_s,
223        double centerOfRotationX,
224        double centerOfRotationY,
225        int driveRequestType,
226        int steerRequestType,
227        boolean desaturateWheelSpeeds,
228        int forwardPerspective);
229    public static native void JNI_SetControl_RobotCentric(
230        int id,
231        double velocityX_mps,
232        double velocityY_mps,
233        double rotationalRate_rad_per_s,
234        double deadband_mps,
235        double rotationalDeadband_rad_per_s,
236        double centerOfRotationX,
237        double centerOfRotationY,
238        int driveRequestType,
239        int steerRequestType,
240        boolean desaturateWheelSpeeds);
241    public static native int JNI_Request_Apply_RobotCentric(
242        int id,
243        double velocityX_mps,
244        double velocityY_mps,
245        double rotationalRate_rad_per_s,
246        double deadband_mps,
247        double rotationalDeadband_rad_per_s,
248        double centerOfRotationX,
249        double centerOfRotationY,
250        int driveRequestType,
251        int steerRequestType,
252        boolean desaturateWheelSpeeds);
253    public static native void JNI_SetControl_PointWheelsAt(
254        int id,
255        double moduleDirection,
256        int driveRequestType,
257        int steerRequestType);
258    public static native int JNI_Request_Apply_PointWheelsAt(
259        int id,
260        double moduleDirection,
261        int driveRequestType,
262        int steerRequestType);
263    public static native void JNI_SetControl_ApplyRobotSpeeds(
264        int id,
265        double speedsVx,
266        double speedsVy,
267        double speedsOmega,
268        double[] wheelForceFeedforwardsX_N,
269        double[] wheelForceFeedforwardsY_N,
270        double centerOfRotationX,
271        double centerOfRotationY,
272        int driveRequestType,
273        int steerRequestType,
274        boolean desaturateWheelSpeeds);
275    public static native int JNI_Request_Apply_ApplyRobotSpeeds(
276        int id,
277        double speedsVx,
278        double speedsVy,
279        double speedsOmega,
280        double[] wheelForceFeedforwardsX_N,
281        double[] wheelForceFeedforwardsY_N,
282        double centerOfRotationX,
283        double centerOfRotationY,
284        int driveRequestType,
285        int steerRequestType,
286        boolean desaturateWheelSpeeds);
287    public static native void JNI_SetControl_ApplyFieldSpeeds(
288        int id,
289        double speedsVx,
290        double speedsVy,
291        double speedsOmega,
292        double[] wheelForceFeedforwardsX_N,
293        double[] wheelForceFeedforwardsY_N,
294        double centerOfRotationX,
295        double centerOfRotationY,
296        int driveRequestType,
297        int steerRequestType,
298        boolean desaturateWheelSpeeds,
299        int forwardPerspective);
300    public static native int JNI_Request_Apply_ApplyFieldSpeeds(
301        int id,
302        double speedsVx,
303        double speedsVy,
304        double speedsOmega,
305        double[] wheelForceFeedforwardsX_N,
306        double[] wheelForceFeedforwardsY_N,
307        double centerOfRotationX,
308        double centerOfRotationY,
309        int driveRequestType,
310        int steerRequestType,
311        boolean desaturateWheelSpeeds,
312        int forwardPerspective);
313}