Updating mercury app with code for driving steppers

nwdepatie · Mar 29, 2024 · b1745c9 · b1745c9
1 parent bcff225
commit b1745c9
Show file tree

Hide file tree

Showing 4 changed files with 114 additions and 25 deletions.
diff --git a/mercury-app/src/hw_ifc/src/gantry_sub.rs b/mercury-app/src/hw_ifc/src/gantry_sub.rs
@@ -1,11 +1,13 @@
 /* Subscriber for Carrying out Commands for Chassis Drive Motors */
 use rosrust;
+use rosrust::ros_info;
 pub mod gantry_model;
 use self::gantry_model::{
     GantryAxes, GantryModel, GantryPosition, StepperCtrlCmd, StepperCtrlCmdGroup,
 };
 use std::sync::{Arc, Mutex};
 pub mod zynq;
+use rosrust_msg::geometry_msgs::Twist;
 use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};
 
 pub mod msg {
@@ -17,20 +19,22 @@ pub mod msg {
 
 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,
+    GANTRY_DIR1_ADDR, GANTRY_DIR2_ADDR, GANTRY_DIR3_ADDR, GANTRY_DIR4_ADDR, GANTRY_RESET_ADDR, GANTRY_STEP1_ADDR,
+    GANTRY_STEP2_ADDR, GANTRY_STEP3_ADDR, GANTRY_STEP4_ADDR,
 };
 
 pub struct StepperController {
     direction_control: AXIGPIO,
     step_control: AXIGPIO,
+    pub is_running : bool
 }
 
 impl StepperController {
     pub fn new(direction_control: AXIGPIO, step_control: AXIGPIO) -> StepperController {
         StepperController {
             direction_control: direction_control,
             step_control: step_control,
+            is_running : false
         }
     }
 
@@ -43,13 +47,18 @@ impl StepperController {
         self.step_control
             .write_gpio(num_steps, GPIOChannel::GpioChannel1);
     }
+
+    pub fn read_done(&mut self) -> u32 {
+        self.step_control.read_gpio(GPIOChannel::GpioChannel2)
+    }
 }
 
 pub struct GantryController {
     model: GantryModel,
     stepper1: Arc<Mutex<StepperController>>,
     stepper2: Arc<Mutex<StepperController>>,
     stepper3: Arc<Mutex<StepperController>>,
+    stepper4: Arc<Mutex<StepperController>>,
     reset: Arc<Mutex<AXIGPIO>>,
 }
 
@@ -58,13 +67,15 @@ impl GantryController {
         stepper1: Arc<Mutex<StepperController>>,
         stepper2: Arc<Mutex<StepperController>>,
         stepper3: Arc<Mutex<StepperController>>,
+        stepper4: Arc<Mutex<StepperController>>,
         reset: Arc<Mutex<AXIGPIO>>,
     ) -> Self {
         GantryController {
             model: GantryModel::new(),
             stepper1: stepper1,
             stepper2: stepper2,
             stepper3: stepper3,
+            stepper4: stepper4,
             reset: reset,
         }
     }
@@ -115,6 +126,46 @@ impl GantryController {
 
         self.write_steppers(stepper_cmds);
     }
+
+    fn cont_write_stepper(stepper : Arc<Mutex<StepperController>>, val : f64) {
+        if (stepper.lock().unwrap().read_done() == 0) & stepper.lock().unwrap().is_running {
+            return;
+        }
+        else if (stepper.lock().unwrap().read_done() == 0) & !stepper.lock().unwrap().is_running {
+            stepper.lock().unwrap().set_direction(val < 0.0);
+            stepper
+                .lock()
+                .unwrap()
+                .write_step_cmd(f64::abs(val * 10.0) as u32);
+            stepper.lock().unwrap().is_running = true;
+            return;
+        }
+        else if stepper.lock().unwrap().read_done() != 0  {
+            stepper
+                .lock()
+                .unwrap()
+                .write_step_cmd(0);
+            stepper.lock().unwrap().is_running = false;
+            return;
+        }
+    }
+
+    fn command_callback(&self, twist: Twist) {
+        /* Directly take the linear and angular speeds without clamping */
+        let x = twist.linear.x;
+        let y = twist.linear.y;
+        let z = twist.linear.z;
+        let z_prime = twist.angular.z;
+
+        ros_info!("X:{}, Y:{}, Z:{}, Z'{}", x, y, z, z_prime);
+
+        // Clone the Arc before using it. This increases the reference count
+        // but does not clone the underlying Mutex or StepperController.
+        Self::cont_write_stepper(Arc::clone(&self.stepper1), x);
+        Self::cont_write_stepper(Arc::clone(&self.stepper2), y);
+        Self::cont_write_stepper(Arc::clone(&self.stepper3), z);
+        Self::cont_write_stepper(Arc::clone(&self.stepper4), z_prime);
+    }
 }
 
 fn main() {
@@ -136,12 +187,17 @@ fn main() {
         AXIGPIO::new(GANTRY_STEP3_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
     )));
 
+    let stepper4 = Arc::new(Mutex::new(StepperController::new(
+        AXIGPIO::new(GANTRY_DIR4_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+        AXIGPIO::new(GANTRY_STEP4_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,
+        stepper1, stepper2, stepper3, stepper4, reset,
     )));
 
     /* Create subscriber */
@@ -170,6 +226,14 @@ fn main() {
     )
     .unwrap();
 
+    let gantry_ctrl_clone = gantry_ctrl.clone();
+    let _subscriber_info =
+        rosrust::subscribe_with_ids("/gantry/cmd_vel", 2, move |v: Twist, _caller_id: &str| {
+            let mut gantry_ctrl = gantry_ctrl_clone.lock().unwrap();
+            gantry_ctrl.command_callback(v);
+        })
+        .unwrap();
+
     while rosrust::is_ok() {
         /* Spin forever, we only execute things on callbacks from here */
         rosrust::spin();

diff --git a/mercury-app/src/hw_ifc/src/regmap.rs b/mercury-app/src/hw_ifc/src/regmap.rs
@@ -1,13 +1,15 @@
 pub const DRV_DIR_GPIO_ADDR: u32 = 0x7FFF_8000;
-pub const DRV_TIMER_FRONT_RIGHT_ADDR: u32 = 0x4000_0000;
-pub const DRV_TIMER_FRONT_LEFT_ADDR: u32 = 0x4001_0000;
-pub const DRV_TIMER_BACK_RIGHT_ADDR: u32 = 0x4002_0000;
-pub const DRV_TIMER_BACK_LEFT_ADDR: u32 = 0x4003_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 = 0x4004_0000; // TODO: Double check Gantry DIR 1 behavior
-pub const GANTRY_DIR2_ADDR: u32 = 0x4005_0000;
-pub const GANTRY_DIR3_ADDR: u32 = 0x4006_0000;
-pub const GANTRY_RESET_ADDR: u32 = 0x4007_0000;
-pub const GANTRY_STEP1_ADDR: u32 = 0x4008_0000;
-pub const GANTRY_STEP2_ADDR: u32 = 0x4009_0000;
-pub const GANTRY_STEP3_ADDR: u32 = 0x400A_0000;
+pub const GANTRY_DIR1_ADDR: u32 = 0x4120_0000; // TODO: Double check Gantry DIR 1 behavior
+pub const GANTRY_DIR2_ADDR: u32 = 0x4121_0000;
+pub const GANTRY_DIR3_ADDR: u32 = 0x4122_0000;
+pub const GANTRY_DIR4_ADDR: u32 = 0x4123_0000;
+pub const GANTRY_RESET_ADDR: u32 = 0x4124_0000;
+pub const GANTRY_STEP1_ADDR: u32 = 0x4125_0000;
+pub const GANTRY_STEP2_ADDR: u32 = 0x4126_0000;
+pub const GANTRY_STEP3_ADDR: u32 = 0x4127_0000;
+pub const GANTRY_STEP4_ADDR: u32 = 0x4128_0000;
diff --git a/mercury-app/src/hw_ifc/src/step_test.rs b/mercury-app/src/hw_ifc/src/step_test.rs
@@ -5,18 +5,18 @@ use self::gantry_model::{
 };
 use std::sync::{Arc, Mutex};
 pub mod zynq;
+use crate::zynq::axigpio::GPIOChannel;
 use zynq::axigpio::{
     GPIOChannel::GpioChannel1, GPIOChannel::GpioChannel2, AXIGPIO, SIZEOF_AXIGPIO_REG,
 };
-use crate::zynq::axigpio::GPIOChannel;
 
 use std::thread;
 use std::time::Duration;
 
 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,
+    GANTRY_DIR1_ADDR, GANTRY_DIR2_ADDR, GANTRY_DIR3_ADDR, GANTRY_DIR4_ADDR, GANTRY_RESET_ADDR, GANTRY_STEP1_ADDR,
+    GANTRY_STEP2_ADDR, GANTRY_STEP3_ADDR, GANTRY_STEP4_ADDR,
 };
 
 pub struct StepperController {
@@ -41,6 +41,10 @@ impl StepperController {
         self.step_control
             .write_gpio(num_steps, GPIOChannel::GpioChannel1);
     }
+
+    pub fn read_gpio(&mut self) -> u32 {
+        self.step_control.read_gpio(GPIOChannel::GpioChannel2)
+    }
 }
 
 fn main() {
@@ -59,17 +63,26 @@ fn main() {
         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 stepper4 = Arc::new(Mutex::new(StepperController::new(
+        AXIGPIO::new(GANTRY_DIR4_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+        AXIGPIO::new(GANTRY_STEP4_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
+    )));
+
+    stepper4.lock().unwrap().set_direction(true);
+
+    stepper4.lock().unwrap().write_step_cmd(0);
+
+    stepper4.lock().unwrap().write_step_cmd(3000);
+
+    println!("Done: {}", stepper4.lock().unwrap().read_gpio());
 
-    reset.lock().unwrap().write_gpio(1, GpioChannel1);
+    thread::sleep(Duration::from_millis(5000));
 
-    stepper1.lock().unwrap().write_step_cmd(300);
+    stepper4.lock().unwrap().set_direction(false);
 
-    stepper2.lock().unwrap().write_step_cmd(300);
+    stepper4.lock().unwrap().write_step_cmd(0);
 
-    stepper3.lock().unwrap().write_step_cmd(300);
+    stepper4.lock().unwrap().write_step_cmd(3000);
 
-    thread::sleep(Duration::from_secs(2));
+    println!("Done: {}", stepper4.lock().unwrap().read_gpio());
 }
diff --git a/mercury-app/src/hw_ifc/src/zynq/axigpio.rs b/mercury-app/src/hw_ifc/src/zynq/axigpio.rs
@@ -55,5 +55,15 @@ impl AXIGPIO {
         };
 
         self.mapped_mem.write_u32(reg, val);
+        println!("Wrote {}...", self.mapped_mem.read_u32(reg));
+    }
+
+    pub fn read_gpio(&mut self, channel: GPIOChannel) -> u32 {
+        let reg = match channel {
+            GPIOChannel::GpioChannel1 => GPIO_DATA,
+            GPIOChannel::GpioChannel2 => GPIO2_DATA,
+        };
+
+        self.mapped_mem.read_u32(reg)
     }
 }