feat(Motor): add set/get position

vexide · Dec 12, 2024 · 6fe06fb · 6fe06fb
1 parent ece75c5
commit 6fe06fb
Show file tree

Hide file tree

Showing 2 changed files with 98 additions and 40 deletions.
diff --git a/src/main/java/dev/vexide/hydrozoa/devices/smart/Motor.java b/src/main/java/dev/vexide/hydrozoa/devices/smart/Motor.java
@@ -150,6 +150,33 @@ public void setTarget(@NotNull MotorControl target) throws DeviceException {
         this.target = target;
     }
 
+    /**
+     * Gets the current position of the motor.
+     * @return the current position of the motor as an {@link EncoderPosition}
+     * @throws DeviceException if the motor is not connected
+     */
+    public @NotNull EncoderPosition getPosition() throws DeviceException {
+        validateConnection();
+
+        double pos = VexSdk.Motor.vexDeviceMotorPositionGet(port.deviceHandle());
+        return EncoderPosition.ofTicks((int) pos, gearset.getTicksPerRevolution());
+    }
+
+    /**
+     * Sets the current encoder position to the given position without moving the motor.
+     * Analogous to taring or resetting the encoder so that the new position is equal to the given position.
+     * @param position the new position of the motor
+     * @throws DeviceException if the motor is not connected
+     */
+    public void setPosition(@NotNull EncoderPosition position) throws DeviceException {
+        validateConnection();
+
+        VexSdk.Motor.vexDeviceMotorPositionSet(
+                port.deviceHandle(),
+                position.ticks(gearset.getTicksPerRevolution())
+        );
+    }
+
     /**
      * The internal gearset of a V5 Smart Motor.
      */

diff --git a/vex-sdk/src/main/java/dev/vexide/hydrozoa/sdk/VexSdk.java b/vex-sdk/src/main/java/dev/vexide/hydrozoa/sdk/VexSdk.java
@@ -145,46 +145,6 @@ public static final class Motor {
         private Motor() {
         }
 
-        //    TODO:
-        //    vexDeviceMotorZeroPositionFlagGet(device: V5_DeviceT) -> bool,
-        //    vexDeviceMotorReverseFlagGet(device: V5_DeviceT) -> bool,
-        //    vexDeviceMotorEncoderUnitsGet(device: V5_DeviceT) -> V5MotorEncoderUnits,
-        //    vexDeviceMotorBrakeModeGet(device: V5_DeviceT) -> V5MotorBrakeMode,
-        //    vexDeviceMotorPositionSet(device: V5_DeviceT, position: c_double),
-        //    vexDeviceMotorPositionGet(device: V5_DeviceT) -> c_double,
-        //    vexDeviceMotorPositionRawGet(
-        //        device: V5_DeviceT,
-        //        timestamp: *mut u32,
-        //    ) -> i32,
-        //    vexDeviceMotorPositionReset(device: V5_DeviceT),
-        //    vexDeviceMotorTargetGet(device: V5_DeviceT) -> c_double,
-        //    vexDeviceMotorServoTargetSet(device: V5_DeviceT, position: c_double),
-        //    vexDeviceMotorAbsoluteTargetSet(
-        //        device: V5_DeviceT,
-        //        position: c_double,
-        //        veloctiy: i32,
-        //    ),
-        //    vexDeviceMotorRelativeTargetSet(
-        //        device: V5_DeviceT,
-        //        position: c_double,
-        //        velocity: i32,
-        //    ),
-        //    vexDeviceMotorFaultsGet(device: V5_DeviceT) -> u32,
-        //    vexDeviceMotorFlagsGet(device: V5_DeviceT) -> u32,
-        //    vexDeviceMotorVoltageSet(device: V5_DeviceT, voltage: i32),
-        //    vexDeviceMotorVoltageGet(device: V5_DeviceT) -> i32,
-        //    vexDeviceMotorGearingSet(device: V5_DeviceT, gearset: V5MotorGearset),
-        //    vexDeviceMotorGearingGet(device: V5_DeviceT) -> V5MotorGearset,
-        //    vexDeviceMotorVoltageLimitSet(device: V5_DeviceT, limit: i32),
-        //    vexDeviceMotorVoltageLimitGet(device: V5_DeviceT) -> i32,
-        //    vexDeviceMotorVelocityUpdate(device: V5_DeviceT, velocity: i32),
-        //    vexDeviceMotorPositionPidSet(device: V5_DeviceT, pid: *mut V5_DeviceMotorPid),
-        //    vexDeviceMotorVelocityPidSet(device: V5_DeviceT, pid: *mut V5_DeviceMotorPid),
-        //    vexDeviceMotorExternalProfileSet(
-        //        device: V5_DeviceT,
-        //        position: c_double,
-        //        velocity: i32,
-        //    ),
         public static void vexDeviceMotorVelocitySet(@NotNull V5_Device device, int velocity) {
             vexDeviceMotorVelocitySetRaw(device.raw(), velocity);
         }
@@ -363,6 +323,77 @@ public static void vexDeviceMotorVoltageGet(@NotNull V5_Device device, int volta
 
         @Import(module = "vex", name = "vexDeviceMotorVoltageGet")
         private static native void vexDeviceMotorVoltageGetRaw(int device, int voltage);
+        //    TODO:
+        //    vexDeviceMotorPositionGet(device: V5_DeviceT) -> c_double,
+        //    vexDeviceMotorPositionRawGet(
+        //        device: V5_DeviceT,
+        //        timestamp: *mut u32,
+        //    ) -> i32,
+        //    vexDeviceMotorPositionReset(device: V5_DeviceT),
+        //    vexDeviceMotorTargetGet(device: V5_DeviceT) -> c_double,
+        //    vexDeviceMotorServoTargetSet(device: V5_DeviceT, position: c_double),
+        //    vexDeviceMotorAbsoluteTargetSet(
+        //        device: V5_DeviceT,
+        //        position: c_double,
+        //        veloctiy: i32,
+        //    ),
+        //    vexDeviceMotorRelativeTargetSet(
+        //        device: V5_DeviceT,
+        //        position: c_double,
+        //        velocity: i32,
+        //    ),
+        //    vexDeviceMotorFaultsGet(device: V5_DeviceT) -> u32,
+        //    vexDeviceMotorFlagsGet(device: V5_DeviceT) -> u32,
+        //    vexDeviceMotorVoltageSet(device: V5_DeviceT, voltage: i32),
+        //    vexDeviceMotorVoltageGet(device: V5_DeviceT) -> i32,
+        //    vexDeviceMotorGearingSet(device: V5_DeviceT, gearset: V5MotorGearset),
+        //    vexDeviceMotorGearingGet(device: V5_DeviceT) -> V5MotorGearset,
+        //    vexDeviceMotorVoltageLimitSet(device: V5_DeviceT, limit: i32),
+        //    vexDeviceMotorVoltageLimitGet(device: V5_DeviceT) -> i32,
+        //    vexDeviceMotorVelocityUpdate(device: V5_DeviceT, velocity: i32),
+        //    vexDeviceMotorPositionPidSet(device: V5_DeviceT, pid: *mut V5_DeviceMotorPid),
+        //    vexDeviceMotorVelocityPidSet(device: V5_DeviceT, pid: *mut V5_DeviceMotorPid),
+        //    vexDeviceMotorExternalProfileSet(
+        //        device: V5_DeviceT,
+        //        position: c_double,
+        //        velocity: i32,
+        //    ),
+
+
+        public static boolean vexDeviceMotorReverseFlagGet(@NotNull V5_Device device) {
+            return vexDeviceMotorReverseFlagGetRaw(device.raw());
+        }
+
+        @Import(module = "vex", name = "vexDeviceMotorReverseFlagGet")
+        private static native boolean vexDeviceMotorReverseFlagGetRaw(int device);
+
+        public static @NotNull V5MotorEncoderUnits vexDeviceMotorEncoderUnitsGet(@NotNull V5_Device device) {
+            return new V5MotorEncoderUnits(vexDeviceMotorEncoderUnitsGetRaw(device.raw()));
+        }
+
+        @Import(module = "vex", name = "vexDeviceMotorEncoderUnitsGet")
+        private static native byte vexDeviceMotorEncoderUnitsGetRaw(int device);
+
+        public static @NotNull V5MotorBrakeMode vexDeviceMotorBrakeModeGet(@NotNull V5_Device device) {
+            return new V5MotorBrakeMode(vexDeviceMotorBrakeModeGetRaw(device.raw()));
+        }
+
+        @Import(module = "vex", name = "vexDeviceMotorBrakeModeGet")
+        private static native byte vexDeviceMotorBrakeModeGetRaw(int device);
+
+        public static void vexDeviceMotorPositionSet(@NotNull V5_Device device, double position) {
+            vexDeviceMotorPositionSetRaw(device.raw(), position);
+        }
+
+        @Import(module = "vex", name = "vexDeviceMotorPositionSet")
+        private static native void vexDeviceMotorPositionSetRaw(int device, double position);
+
+        public static double vexDeviceMotorPositionGet(@NotNull V5_Device device) {
+            return vexDeviceMotorPositionGetRaw(device.raw());
+        }
+
+        @Import(module = "vex", name = "vexDeviceMotorPositionGet")
+        private static native double vexDeviceMotorPositionGetRaw(int device);
     }
 
     @StaticInit