Updating Rust code with new drive enable logic

nwdepatie · Apr 3, 2024 · 7725850 · 7725850
1 parent 1015ea3
commit 7725850
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 21 deletions.
diff --git a/mercury-app/src/hw_ifc/src/drive_sub.rs b/mercury-app/src/hw_ifc/src/drive_sub.rs
@@ -8,7 +8,7 @@ pub mod zynq;
 use self::chassis_model::{ChassisModel, MotorCtrlCmdGroup, WheelPositions};
 use self::regmap::{
     DRV_DIR_GPIO_ADDR, DRV_TIMER_BACK_LEFT_ADDR, DRV_TIMER_BACK_RIGHT_ADDR,
-    DRV_TIMER_FRONT_LEFT_ADDR, DRV_TIMER_FRONT_RIGHT_ADDR,
+    DRV_TIMER_FRONT_LEFT_ADDR, DRV_TIMER_FRONT_RIGHT_ADDR, DRV_DRIVE_ENABLE_ADDR,
 };
 use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};
 use zynq::axitimer::{AXITimer, SIZEOF_AXITIMER_REG};
@@ -21,6 +21,7 @@ const BOT_LENGTH: f32 = 0.20; /* meters */
 
 type PWMController = Arc<Mutex<AXITimer>>;
 type DirectionController = Arc<Mutex<AXIGPIO>>;
+type DriveEnable = Arc<Mutex<AXIGPIO>>;
 
 pub struct DriveController {
     model: ChassisModel,
@@ -29,6 +30,7 @@ pub struct DriveController {
     pwm_back_left: PWMController,
     pwm_back_right: PWMController,
     direction_ctrl: DirectionController,
+    drv_enable: DriveEnable,
 }
 
 impl DriveController {
@@ -38,6 +40,7 @@ impl DriveController {
         pwm_back_left: PWMController,
         pwm_back_right: PWMController,
         direction_ctrl: DirectionController,
+        drv_enable: DriveEnable,
     ) -> Self {
         /* Parameters */
         let wheel_radius = WHEEL_RADIUS;
@@ -58,6 +61,7 @@ impl DriveController {
             pwm_back_left,
             pwm_back_right,
             direction_ctrl,
+            drv_enable,
         }
     }
 
@@ -75,6 +79,19 @@ impl DriveController {
             GPIOChannel::GpioChannel2,
         );
 
+        if ((left_pwm_u32 == 0) | (right_pwm_u32 == 0)) {
+            self.drv_enable.lock().unwrap().write_gpio(
+                0,
+                GPIOChannel::GpioChannel1,
+            );
+        }
+        else {
+            self.drv_enable.lock().unwrap().write_gpio(
+                1,
+                GPIOChannel::GpioChannel1,
+            );
+        }
+
         /* Set PWM Duty Cycles */
         self.pwm_front_left
             .lock()
@@ -134,13 +151,17 @@ fn main() {
     let direction_control = Arc::new(Mutex::new(
         AXIGPIO::new(DRV_DIR_GPIO_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
     ));
+    let drive_enable = Arc::new(Mutex::new(
+        AXIGPIO::new(DRV_DRIVE_ENABLE_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+    ));
 
     let drive_ctrl = DriveController::new(
         pwm_front_right,
         pwm_front_left,
         pwm_back_right,
         pwm_back_left,
         direction_control,
+        drive_enable
     );
 
     /*

diff --git a/mercury-app/src/hw_ifc/src/regmap.rs b/mercury-app/src/hw_ifc/src/regmap.rs
@@ -1,22 +1,23 @@
-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_DIR_GPIO_ADDR: u32 = 0x4121_8000;
+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 = 0x4122_0000;
 
-pub const GANTRY_DIR1_ADDR: u32 = 0x4006_0000;
-pub const GANTRY_DIR2_ADDR: u32 = 0x4007_0000;
-pub const GANTRY_DIR3_ADDR: u32 = 0x4008_0000;
-pub const GANTRY_DIR4_ADDR: u32 = 0x4009_0000;
-pub const GANTRY_DIR5_ADDR: u32 = 0x400A_0000;
-pub const GANTRY_RESET_ADDR: u32 = 0x400B_0000;
-pub const GANTRY_STEP1_ADDR: u32 = 0x400C_0000;
-pub const GANTRY_STEP2_ADDR: u32 = 0x400D_0000;
-pub const GANTRY_STEP3_ADDR: u32 = 0x400E_0000;
-pub const GANTRY_STEP4_ADDR: u32 = 0x400F_0000;
-pub const GANTRY_STEP5_ADDR: u32 = 0x4010_0000;
+pub const GANTRY_DIR1_ADDR: u32 = 0x4123_0000;
+pub const GANTRY_DIR2_ADDR: u32 = 0x4124_0000;
+pub const GANTRY_DIR3_ADDR: u32 = 0x4125_0000;
+pub const GANTRY_DIR4_ADDR: u32 = 0x4126_0000;
+pub const GANTRY_DIR5_ADDR: u32 = 0x4127_0000;
+pub const GANTRY_RESET_ADDR: u32 = 0x4128_0000;
+pub const GANTRY_STEP1_ADDR: u32 = 0x4129_0000;
+pub const GANTRY_STEP2_ADDR: u32 = 0x412A_0000;
+pub const GANTRY_STEP3_ADDR: u32 = 0x412B_0000;
+pub const GANTRY_STEP4_ADDR: u32 = 0x412C_0000;
+pub const GANTRY_STEP5_ADDR: u32 = 0x412D_0000;
 
-pub const LED_BRAKELIGHT: u32 = 0x4011_0000;
-pub const LED_FRONT_LOWER: u32 = 0x4012_0000;
-pub const LED_FRONT_UPPER: u32 = 0x4013_0000;
-pub const LED_WORKCELL: u32 = 0x4014_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;