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