Integrating AXI peripherals into gantry sub

nwdepatie · Mar 24, 2024 · 1e47fb7 · 1e47fb7
1 parent 0d0ed2f
commit 1e47fb7
Show file tree

Hide file tree

Showing 4 changed files with 127 additions and 35 deletions.
diff --git a/mercury-app/src/hw_ifc/src/drive_sub.rs b/mercury-app/src/hw_ifc/src/drive_sub.rs
@@ -3,17 +3,16 @@
 use rosrust_msg::geometry_msgs::Twist;
 use std::sync::{Arc, Mutex};
 pub mod chassis_model;
+pub mod regmap;
 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,
+};
 use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};
 use zynq::axitimer::{AXITimer, SIZEOF_AXITIMER_REG};
 
-const TIMER_FRONT_RIGHT_ADDR: u32 = 0x4280_0000;
-const TIMER_FRONT_LEFT_ADDR: u32 = 0x4281_0000;
-const TIMER_BACK_RIGHT_ADDR: u32 = 0x4282_0000;
-const TIMER_BACK_LEFT_ADDR: u32 = 0x4283_0000;
-const DIRECTION_GPIO_ADDR: u32 = 0x4121_0000;
-
 const MAX_LINEAR_SPEED: f64 = 10.0; /* meters/second */
 const MAX_ANGULAR_SPEED: f64 = 10.0; /* radians/second */
 const WHEEL_RADIUS: f64 = 10.0; /* meters */
@@ -123,19 +122,19 @@ fn main() {
     rosrust::init("drive_sub");
 
     let pwm_front_right = Arc::new(Mutex::new(
-        AXITimer::new(TIMER_FRONT_RIGHT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
+        AXITimer::new(DRV_TIMER_FRONT_RIGHT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
     ));
     let pwm_front_left = Arc::new(Mutex::new(
-        AXITimer::new(TIMER_FRONT_LEFT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
+        AXITimer::new(DRV_TIMER_FRONT_LEFT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
     ));
     let pwm_back_right = Arc::new(Mutex::new(
-        AXITimer::new(TIMER_BACK_RIGHT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
+        AXITimer::new(DRV_TIMER_BACK_RIGHT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
     ));
     let pwm_back_left = Arc::new(Mutex::new(
-        AXITimer::new(TIMER_BACK_LEFT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
+        AXITimer::new(DRV_TIMER_BACK_LEFT_ADDR, SIZEOF_AXITIMER_REG).unwrap(),
     ));
     let direction_control = Arc::new(Mutex::new(
-        AXIGPIO::new(DIRECTION_GPIO_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+        AXIGPIO::new(DRV_DIR_GPIO_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
     ));
 
     let drive_ctrl = DriveController::new(

diff --git a/mercury-app/src/hw_ifc/src/gantry_model.rs b/mercury-app/src/hw_ifc/src/gantry_model.rs
@@ -10,7 +10,6 @@ pub enum GantryAxes {
 
 pub struct StepperCtrlCmd {
     pub steps: i32,
-    speed: f64,
 }
 
 pub struct StepperCtrlCmdGroup {
@@ -77,18 +76,16 @@ impl GantryPosition {
 
 pub struct GantryModel {
     current_position: GantryPosition,
-    max_speed: f64,
 }
 
 impl GantryModel {
-    pub fn new(max_speed: f64) -> GantryModel {
+    pub fn new() -> GantryModel {
         GantryModel {
             current_position: GantryPosition {
                 x: 0.0,
                 y: 0.0,
                 z: 0.0,
             },
-            max_speed: max_speed,
         }
     }
 
@@ -109,18 +106,9 @@ impl GantryModel {
         self.current_position = target_position;
 
         StepperCtrlCmdGroup {
-            x: StepperCtrlCmd {
-                steps: x_steps,
-                speed: self.max_speed,
-            },
-            y: StepperCtrlCmd {
-                steps: y_steps,
-                speed: self.max_speed,
-            },
-            z: StepperCtrlCmd {
-                steps: z_steps,
-                speed: self.max_speed,
-            },
+            x: StepperCtrlCmd { steps: x_steps },
+            y: StepperCtrlCmd { steps: y_steps },
+            z: StepperCtrlCmd { steps: z_steps },
         }
     }
 }
diff --git a/mercury-app/src/hw_ifc/src/gantry_sub.rs b/mercury-app/src/hw_ifc/src/gantry_sub.rs
@@ -5,50 +5,142 @@ use self::gantry_model::{
     GantryAxes, GantryModel, GantryPosition, StepperCtrlCmd, StepperCtrlCmdGroup,
 };
 use std::sync::{Arc, Mutex};
+pub mod zynq;
+use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};
 
 pub mod msg {
     rosrust::rosmsg_include!(hw_srv / movegantry, hw_srv / calibrategantry);
 }
 
-const MAX_SPEED: f64 = 10.0; /* meters/second */
+pub mod regmap;
+use self::regmap::{
+    GANTRY_DIR1_ADDR, GANTRY_DIR2_ADDR, GANTRY_DIR3_ADDR, GANTRY_RESET_ADDR, GANTRY_STEP1_ADDR,
+    GANTRY_STEP2_ADDR, GANTRY_STEP3_ADDR,
+};
+
+pub struct StepperController {
+    direction_control: AXIGPIO,
+    step_control: AXIGPIO,
+}
+
+impl StepperController {
+    pub fn new(direction_control: AXIGPIO, step_control: AXIGPIO) -> StepperController {
+        StepperController {
+            direction_control: direction_control,
+            step_control: step_control,
+        }
+    }
+
+    pub fn set_direction(&mut self, is_forward: bool) {
+        self.direction_control
+            .write_gpio(is_forward as u32, GPIOChannel::GpioChannel1);
+    }
+
+    pub fn write_step_cmd(&mut self, num_steps: u32) {
+        self.step_control
+            .write_gpio(num_steps, GPIOChannel::GpioChannel1);
+    }
+}
 
 pub struct GantryController {
     model: GantryModel,
+    stepper1: Arc<Mutex<StepperController>>,
+    stepper2: Arc<Mutex<StepperController>>,
+    stepper3: Arc<Mutex<StepperController>>,
+    reset: Arc<Mutex<AXIGPIO>>,
 }
 
 impl GantryController {
-    pub fn new() -> Self {
+    pub fn new(
+        stepper1: Arc<Mutex<StepperController>>,
+        stepper2: Arc<Mutex<StepperController>>,
+        stepper3: Arc<Mutex<StepperController>>,
+        reset: Arc<Mutex<AXIGPIO>>,
+    ) -> Self {
         GantryController {
-            model: GantryModel::new(MAX_SPEED),
+            model: GantryModel::new(),
+            stepper1: stepper1,
+            stepper2: stepper2,
+            stepper3: stepper3,
+            reset: reset,
         }
     }
 
+    fn write_steppers(&mut self, stepper_cmds: StepperCtrlCmdGroup) {
+        self.stepper1
+            .lock()
+            .unwrap()
+            .set_direction(stepper_cmds[GantryAxes::X].steps < 0);
+        self.stepper1
+            .lock()
+            .unwrap()
+            .write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
+        self.stepper2
+            .lock()
+            .unwrap()
+            .set_direction(stepper_cmds[GantryAxes::Y].steps < 0);
+        self.stepper2
+            .lock()
+            .unwrap()
+            .write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Y].steps) as u32);
+        self.stepper3
+            .lock()
+            .unwrap()
+            .set_direction(stepper_cmds[GantryAxes::Z].steps < 0);
+        self.stepper3
+            .lock()
+            .unwrap()
+            .write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Z].steps) as u32);
+    }
+
     pub fn move_callback(&mut self, data: rosrust_msg::hw_srv::movegantryReq) {
         let stepper_cmds = self
             .model
             .calc_control_signals(GantryPosition::new(data.x, data.y, data.z));
-        //stepper_cmds[GantryAxes::X];
-        //stepper_cmds[GantryAxes::Y];
-        //stepper_cmds[GantryAxes::Z];
+
+        self.write_steppers(stepper_cmds);
     }
 
     pub fn calibrate_callback(&mut self) {
         let stepper_cmds = self
             .model
             .calc_control_signals(GantryPosition::new(0.0, 0.0, 0.0));
+
+        self.write_steppers(stepper_cmds);
     }
 }
 
 fn main() {
     /* Initialize ROS node */
     rosrust::init("gantry_sub");
 
-    let gantry_ctrl = Arc::new(Mutex::new(GantryController::new()));
+    let stepper1 = Arc::new(Mutex::new(StepperController::new(
+        AXIGPIO::new(GANTRY_DIR1_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+        AXIGPIO::new(GANTRY_STEP1_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+    )));
+
+    let stepper2 = Arc::new(Mutex::new(StepperController::new(
+        AXIGPIO::new(GANTRY_DIR2_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+        AXIGPIO::new(GANTRY_STEP2_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+    )));
+
+    let stepper3 = Arc::new(Mutex::new(StepperController::new(
+        AXIGPIO::new(GANTRY_DIR3_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+        AXIGPIO::new(GANTRY_STEP3_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+    )));
+
+    let reset = Arc::new(Mutex::new(
+        AXIGPIO::new(GANTRY_DIR3_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+    ));
+
+    let gantry_ctrl = Arc::new(Mutex::new(GantryController::new(
+        stepper1, stepper2, stepper3, reset,
+    )));
 
     /* Create subscriber */
     let gantry_ctrl_clone = gantry_ctrl.clone();
     let _service_move = rosrust::service::<rosrust_msg::hw_srv::movegantry, _>(
-        "/gantry/pose/goal",
+        "/services/gantry/setgoal",
         move |coords: rosrust_msg::hw_srv::movegantryReq| {
             let mut gantry_ctrl = gantry_ctrl_clone.lock().unwrap();
             gantry_ctrl.move_callback(coords);

diff --git a/mercury-app/src/hw_ifc/src/regmap.rs b/mercury-app/src/hw_ifc/src/regmap.rs
@@ -0,0 +1,13 @@
+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 GANTRY_DIR1_ADDR: u32 = 0x8000_0000; // TODO: Double check Gantry DIR 1 behavior
+pub const GANTRY_DIR2_ADDR: u32 = 0x4120_0000;
+pub const GANTRY_DIR3_ADDR: u32 = 0x4121_0000;
+pub const GANTRY_RESET_ADDR: u32 = 0x4126_0000;
+pub const GANTRY_STEP1_ADDR: u32 = 0x4123_0000;
+pub const GANTRY_STEP2_ADDR: u32 = 0x4124_0000;
+pub const GANTRY_STEP3_ADDR: u32 = 0x4125_0000;