Gantry Control

mercury-app/src/hw_ifc/CMakeLists.txt

@@ -151,7 +151,7 @@ include_directories(
 
 add_custom_target(hw_ifc
 	ALL
-	COMMAND cargo build -p mercury --bin drive_sub #--release
+	COMMAND cargo build -p mercury --release #--target armv7-unknown-linux-gnueabihf
   WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/../src/hw_ifc
 	#COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_BINARY_DIR}/cargo/release/my-project-binary ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/my-project-binary
 	COMMENT "Build Mercury..."

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(

mercury-app/src/hw_ifc/src/gantry_model.rs

@@ -1,22 +1,114 @@
-pub struct DriveMotorModel {
-	wheel_base: f64,
-	wheel_radius: f64,
+/* Model Describing the Behavior of the Gantry */
+
+use std::ops::{Index, IndexMut};
+
+pub enum GantryAxes {
+    X,
+    Y,
+    Z,
+}
+
+pub struct StepperCtrlCmd {
+    pub steps: i32,
+}
+
+pub struct StepperCtrlCmdGroup {
+    x: StepperCtrlCmd,
+    y: StepperCtrlCmd,
+    z: StepperCtrlCmd,
+}
+
+impl Index<GantryAxes> for StepperCtrlCmdGroup {
+    type Output = StepperCtrlCmd;
+
+    fn index(&self, axis: GantryAxes) -> &Self::Output {
+        match axis {
+            GantryAxes::X => &self.x,
+            GantryAxes::Y => &self.y,
+            GantryAxes::Z => &self.z,
+        }
+    }
+}
+
+impl IndexMut<GantryAxes> for StepperCtrlCmdGroup {
+    fn index_mut(&mut self, axis: GantryAxes) -> &mut Self::Output {
+        match axis {
+            GantryAxes::X => &mut self.x,
+            GantryAxes::Y => &mut self.y,
+            GantryAxes::Z => &mut self.z,
+        }
+    }
+}
+
+pub struct GantryPosition {
+    x: f64,
+    y: f64,
+    z: f64,
 }
 
-impl DriveMotorModel {
-    pub fn new(wheel_base : f64, wheel_radius : f64) -> DriveMotorModel{
-        DriveMotorModel {
-            wheel_base,
-            wheel_radius
+impl Index<GantryAxes> for GantryPosition {
+    type Output = f64;
+
+    fn index(&self, axis: GantryAxes) -> &Self::Output {
+        match axis {
+            GantryAxes::X => &self.x,
+            GantryAxes::Y => &self.y,
+            GantryAxes::Z => &self.z,
         }
     }
+}
 
-    /* Immutable access */
-    pub fn wheel_base(&self) -> &f64 {
-        &self.wheel_base
+impl IndexMut<GantryAxes> for GantryPosition {
+    fn index_mut(&mut self, axis: GantryAxes) -> &mut Self::Output {
+        match axis {
+            GantryAxes::X => &mut self.x,
+            GantryAxes::Y => &mut self.y,
+            GantryAxes::Z => &mut self.z,
+        }
     }
+}
+
+impl GantryPosition {
+    pub fn new(x: f64, y: f64, z: f64) -> GantryPosition {
+        GantryPosition { x: x, y: y, z: z }
+    }
+}
+
+pub struct GantryModel {
+    current_position: GantryPosition,
+}
 
-    pub fn wheel_radius(&self) -> &f64 {
-        &self.wheel_radius
+impl GantryModel {
+    pub fn new() -> GantryModel {
+        GantryModel {
+            current_position: GantryPosition {
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+            },
+        }
+    }
+
+    pub fn set_position(&mut self, pos: GantryPosition) {
+        self.current_position = pos;
+    }
+
+    pub fn calc_control_signals(&mut self, target_position: GantryPosition) -> StepperCtrlCmdGroup {
+        // assuming 1 unit = 1 step for simplicity
+        let x_steps =
+            (target_position[GantryAxes::X] - self.current_position[GantryAxes::X]) as i32;
+        let y_steps =
+            (target_position[GantryAxes::Y] - self.current_position[GantryAxes::Y]) as i32;
+        let z_steps =
+            (target_position[GantryAxes::Z] - self.current_position[GantryAxes::Z]) as i32;
+
+        // Update current position
+        self.current_position = target_position;
+
+        StepperCtrlCmdGroup {
+            x: StepperCtrlCmd { steps: x_steps },
+            y: StepperCtrlCmd { steps: y_steps },
+            z: StepperCtrlCmd { steps: z_steps },
+        }
     }
 }