Working Rust for chassis drive

nwdepatie · Apr 5, 2024 · 5c3120c · 5c3120c
1 parent c9d81ae
commit 5c3120c
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 24 deletions.
diff --git a/mercury-app/src/hw_ifc/src/drive_sub.rs b/mercury-app/src/hw_ifc/src/drive_sub.rs
@@ -73,13 +73,16 @@ impl DriveController {
             .dutycycle
             .clamp(0, u32::MAX);
 
+        let left_dir = motor_commands[WheelPositions::LeftWheels].direction as u32;
+        let right_dir = motor_commands[WheelPositions::RightWheels].direction as u32;
+
         /* Set the directions */
         self.direction_ctrl.lock().unwrap().write_gpio(
-            motor_commands[WheelPositions::LeftWheels].direction as u32,
+            (left_dir << 1) | (!left_dir & 0xb1),
             GPIOChannel::GpioChannel1,
         );
         self.direction_ctrl.lock().unwrap().write_gpio(
-            motor_commands[WheelPositions::RightWheels].direction as u32,
+            (right_dir << 1) | (!right_dir & 0xb1),
             GPIOChannel::GpioChannel2,
         );
 

diff --git a/mercury-app/src/hw_ifc/src/regmap.rs b/mercury-app/src/hw_ifc/src/regmap.rs
@@ -1,25 +1,25 @@
-pub const DRV_DIR_GPIO_ADDR: u32 = 0x4001_8000;
-pub const DRV_TIMER_FRONT_RIGHT_ADDR: u32 = 0x4002_0000;
-pub const DRV_TIMER_FRONT_LEFT_ADDR: u32 = 0x4003_0000;
-pub const DRV_TIMER_BACK_RIGHT_ADDR: u32 = 0x4004_0000;
-pub const DRV_TIMER_BACK_LEFT_ADDR: u32 = 0x4005_0000;
-pub const DRV_DRIVE_ENABLE_ADDR: u32 = 0x4006_0000;
+pub const DRV_DIR_GPIO_ADDR: u32 = 0x4122_0000;
+pub const DRV_TIMER_FRONT_RIGHT_ADDR: u32 = 0x4280_0000;
+pub const DRV_TIMER_FRONT_LEFT_ADDR: u32 = 0x4281_0000;
+pub const DRV_TIMER_BACK_RIGHT_ADDR: u32 = 0x4282_0000;
+pub const DRV_TIMER_BACK_LEFT_ADDR: u32 = 0x4283_0000;
+pub const DRV_DRIVE_ENABLE_ADDR: u32 = 0x4123_0000;
 
-pub const GANTRY_DIR1_ADDR: u32 = 0x4007_0000;
-pub const GANTRY_DIR2_ADDR: u32 = 0x4008_0000;
-pub const GANTRY_DIR3_ADDR: u32 = 0x4009_0000;
-pub const GANTRY_DIR4_ADDR: u32 = 0x400A_0000;
-pub const GANTRY_DIR5_ADDR: u32 = 0x400B_0000;
-pub const GANTRY_RESET_ADDR: u32 = 0x400C_0000;
-pub const GANTRY_STEP1_ADDR: u32 = 0x400D_0000;
-pub const GANTRY_STEP2_ADDR: u32 = 0x400E_0000;
-pub const GANTRY_STEP3_ADDR: u32 = 0x400F_0000;
-pub const GANTRY_STEP4_ADDR: u32 = 0x4010_0000;
-pub const GANTRY_STEP5_ADDR: u32 = 0x4011_0000;
+pub const GANTRY_DIR1_ADDR: u32 = 0x4124_0000;
+pub const GANTRY_DIR2_ADDR: u32 = 0x4125_0000;
+pub const GANTRY_DIR3_ADDR: u32 = 0x4126_0000;
+pub const GANTRY_DIR4_ADDR: u32 = 0x4127_0000;
+pub const GANTRY_DIR5_ADDR: u32 = 0x4128_0000;
+pub const GANTRY_RESET_ADDR: u32 = 0x4129_0000;
+pub const GANTRY_STEP1_ADDR: u32 = 0x412A_0000;
+pub const GANTRY_STEP2_ADDR: u32 = 0x412B_0000;
+pub const GANTRY_STEP3_ADDR: u32 = 0x412C_0000;
+pub const GANTRY_STEP4_ADDR: u32 = 0x412D_0000;
+pub const GANTRY_STEP5_ADDR: u32 = 0x412E_0000;
 
-pub const LED_BRAKELIGHT: u32 = 0x4012_0000;
-pub const LED_FRONT_LOWER: u32 = 0x4013_0000;
-pub const LED_FRONT_UPPER: u32 = 0x4014_0000;
-pub const LED_WORKCELL: u32 = 0x4015_0000;
+pub const LED_BRAKELIGHT: u32 = 0x4284_0000;
+pub const LED_FRONT_LOWER: u32 = 0x4285_0000;
+pub const LED_FRONT_UPPER: u32 = 0x4286_0000;
+pub const LED_WORKCELL: u32 = 0x4287_0000;
 
-pub const SERVO_PWM_ADDR: u32 = 0x4280_0000;
+pub const SERVO_PWM_ADDR: u32 = 0x7FFF_8000;