Skip to content

Commit

Permalink
More drive and servo code
Browse files Browse the repository at this point in the history
  • Loading branch information
@nwdepatie
nwdepatie committed Apr 3, 2024
1 parent a035b95 commit b36cf5d
Show file tree
Hide file tree
Showing 7 changed files with 117 additions and 155 deletions.
4 changes: 0 additions & 4 deletions mercury-app/src/hw_ifc/Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,6 @@ path = "src/drive_sub.rs"
name = "led_sub"
path = "src/led_sub.rs"

[[bin]]
name = "eoat_sub"
path = "src/eoat_sub.rs"

[[bin]]
name = "step_test"
path = "src/step_test.rs"
Expand Down
11 changes: 4 additions & 7 deletions mercury-app/src/hw_ifc/src/chassis_model.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,10 +92,7 @@ impl ChassisModel {
.abs()
.max(cmds[WheelPositions::RightWheels].speed.abs());

rosrust::ros_info!(
"Max Speed {}",
max_wheel_speed
);
rosrust::ros_info!("Max Speed {}", max_wheel_speed);

/* Normalize wheel speeds if exceeding MAX_SPEED */
if max_wheel_speed > self.max_speed {
Expand All @@ -111,14 +108,14 @@ impl ChassisModel {
cmds[WheelPositions::RightWheels].speed
);

let rpm_to_rad_per_s = 2.0 * PI / 60.0; // Conversion factor from RPM to rad/s
// Max RPM
let rpm_to_rad_per_s = 2.0 * PI / 60.0; // Conversion factor from RPM to rad/s
// Max RPM
let max_rpm = 4000.0;
let rad_per_s_high = max_rpm * rpm_to_rad_per_s;

/* Convert normalized wheel speeds to PWM duty cycle and direction */
cmds[WheelPositions::LeftWheels].speed /= rad_per_s_high;
cmds[WheelPositions::RightWheels].speed /= rad_per_s_high;
cmds[WheelPositions::RightWheels].speed /= rad_per_s_high;
rosrust::ros_info!(
"Left Vel/RPM Scaled: {} (from 0 to 1), Right Vel/RPM Scaled: {} rad/s",
cmds[WheelPositions::LeftWheels].speed,
Expand Down
33 changes: 18 additions & 15 deletions mercury-app/src/hw_ifc/src/drive_sub.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@ 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, DRV_DRIVE_ENABLE_ADDR,
DRV_DIR_GPIO_ADDR, DRV_DRIVE_ENABLE_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};
Expand Down Expand Up @@ -66,8 +66,12 @@ impl DriveController {
}

fn send_motor_commands(&self, motor_commands: MotorCtrlCmdGroup) {
let left_pwm_u32 = motor_commands[WheelPositions::LeftWheels].dutycycle.clamp(0, u32::MAX);
let right_pwm_u32 = motor_commands[WheelPositions::RightWheels].dutycycle.clamp(0, u32::MAX);
let left_pwm_u32 = motor_commands[WheelPositions::LeftWheels]
.dutycycle
.clamp(0, u32::MAX);
let right_pwm_u32 = motor_commands[WheelPositions::RightWheels]
.dutycycle
.clamp(0, u32::MAX);

/* Set the directions */
self.direction_ctrl.lock().unwrap().write_gpio(
Expand All @@ -80,16 +84,15 @@ impl DriveController {
);

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,
);
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 */
Expand Down Expand Up @@ -161,7 +164,7 @@ fn main() {
pwm_back_right,
pwm_back_left,
direction_control,
drive_enable
drive_enable,
);

/*
Expand Down
70 changes: 0 additions & 70 deletions mercury-app/src/hw_ifc/src/eoat_sub.rs
View file

This file was deleted.

23 changes: 12 additions & 11 deletions mercury-app/src/hw_ifc/src/gantry_model.rs
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
/* Model Describing the Behavior of the Gantry */
use std::ops::{Index, IndexMut};

pub const MM_PER_STEP : f64 = 0.025;
pub const GANTRY_X_MAX_MM : f64 = 160.0;
pub const GANTRY_Y_MAX_MM : f64 = 120.0;
pub const MM_PER_STEP: f64 = 0.025;
pub const GANTRY_X_MAX_MM: f64 = 160.0;
pub const GANTRY_Y_MAX_MM: f64 = 120.0;

pub enum GantryAxes {
X,
Expand Down Expand Up @@ -115,14 +115,15 @@ impl GantryModel {
}

pub fn calc_control_signals(&mut self, target_position: GantryPosition) -> StepperCtrlCmdGroup {
let x_steps =
((target_position[GantryAxes::X] - self.current_position[GantryAxes::X]) / MM_PER_STEP) as i32;
let y_steps =
((target_position[GantryAxes::Y] - self.current_position[GantryAxes::Y]) / MM_PER_STEP) as i32;
let z_steps =
((target_position[GantryAxes::Z] - self.current_position[GantryAxes::Z]) / MM_PER_STEP) as i32;
let z_prime_steps =
((target_position[GantryAxes::ZPrime] - self.current_position[GantryAxes::ZPrime]) / MM_PER_STEP) as i32;
let x_steps = ((target_position[GantryAxes::X] - self.current_position[GantryAxes::X])
/ MM_PER_STEP) as i32;
let y_steps = ((target_position[GantryAxes::Y] - self.current_position[GantryAxes::Y])
/ MM_PER_STEP) as i32;
let z_steps = ((target_position[GantryAxes::Z] - self.current_position[GantryAxes::Z])
/ MM_PER_STEP) as i32;
let z_prime_steps = ((target_position[GantryAxes::ZPrime]
- self.current_position[GantryAxes::ZPrime])
/ MM_PER_STEP) as i32;

// Update current position
self.current_position = target_position;
Expand Down
87 changes: 60 additions & 27 deletions mercury-app/src/hw_ifc/src/gantry_sub.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,17 @@
use rosrust;
use rosrust::ros_info;
pub mod gantry_model;
use self::gantry_model::{GantryAxes, GantryModel, GantryPosition, StepperCtrlCmdGroup, MM_PER_STEP, GANTRY_X_MAX_MM, GANTRY_Y_MAX_MM};
use self::gantry_model::{
GantryAxes, GantryModel, GantryPosition, StepperCtrlCmdGroup, GANTRY_X_MAX_MM, GANTRY_Y_MAX_MM,
MM_PER_STEP,
};
use std::sync::{Arc, Mutex};
pub mod zynq;
use rosrust_msg::geometry_msgs::Twist;
use std::thread;
use std::time::Duration;
use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};
use zynq::axitimer::{AXITimer, SIZEOF_AXITIMER_REG};

pub mod msg {
rosrust::rosmsg_include!(
Expand All @@ -22,8 +26,35 @@ pub mod regmap;
use self::regmap::{
GANTRY_DIR1_ADDR, GANTRY_DIR2_ADDR, GANTRY_DIR3_ADDR, GANTRY_DIR4_ADDR, GANTRY_DIR5_ADDR,
GANTRY_STEP1_ADDR, GANTRY_STEP2_ADDR, GANTRY_STEP3_ADDR, GANTRY_STEP4_ADDR, GANTRY_STEP5_ADDR,
SERVO_PWM_ADDR,
};

pub struct ServoController {
timer : AXITimer,
}

impl ServoController {
pub fn new(timer: AXITimer) -> ServoController {
ServoController {
timer: timer,
}
}

pub fn grip(&mut self) {
self.timer.start_pwm(2000, 54);
thread::sleep(Duration::from_millis(3000));
self.timer.start_pwm(2000, 75);
}

pub fn release(&mut self) {
self.timer.start_pwm(2000, 94);
thread::sleep(Duration::from_millis(5000));
//self.timer.start_pwm(2000, 54);
thread::sleep(Duration::from_millis(50));
self.timer.start_pwm(2000, 75);
}
}

pub struct StepperController {
direction_control: AXIGPIO,
step_control: AXIGPIO,
Expand Down Expand Up @@ -64,6 +95,7 @@ pub struct GantryController {
stepper3: Arc<Mutex<StepperController>>,
stepper4: Arc<Mutex<StepperController>>,
stepper5: Arc<Mutex<StepperController>>,
servo: Arc<Mutex<ServoController>>,
reset: Arc<Mutex<AXIGPIO>>,
}

Expand All @@ -74,6 +106,7 @@ impl GantryController {
stepper3: Arc<Mutex<StepperController>>,
stepper4: Arc<Mutex<StepperController>>,
stepper5: Arc<Mutex<StepperController>>,
servo: Arc<Mutex<ServoController>>,
reset: Arc<Mutex<AXIGPIO>>,
) -> Self {
GantryController {
Expand All @@ -83,6 +116,7 @@ impl GantryController {
stepper3: stepper3,
stepper4: stepper4,
stepper5: stepper5,
servo: servo,
reset: reset,
}
}
Expand Down Expand Up @@ -155,7 +189,7 @@ impl GantryController {
}
}

fn actuate_eoat(&mut self, z_tune : f64) {
fn actuate_eoat(&mut self, z_tune: f64) {
let current_pos = self.model.get_position();

/* Lower z1 axis by 72mm */
Expand Down Expand Up @@ -193,6 +227,7 @@ impl GantryController {
let stepper_cmds = self.model.calc_control_signals(lower_z);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();
self.servo.lock().unwrap().grip();

/* Fully retract z axis (should b 156mm) */
let mut raise_z = current_pos.clone();
Expand All @@ -204,6 +239,7 @@ impl GantryController {
/* Optional, move over trash */

/* Open servo */
self.servo.lock().unwrap().release();

/* Lower z1 by 72mm */
let mut lower_z1 = current_pos.clone();
Expand All @@ -221,26 +257,16 @@ impl GantryController {

pub fn move_callback(&mut self, data: rosrust_msg::mercury_services::movegantryReq) {
/* Zero everything */
let stepper_cmds = self.model.calc_control_signals(GantryPosition::new(
0.0,
0.0,
0.0,
0.0,
));
let stepper_cmds = self
.model
.calc_control_signals(GantryPosition::new(0.0, 0.0, 0.0, 0.0));
self.write_steppers(stepper_cmds);

let stepper_cmds = self.model.calc_control_signals(GantryPosition::new(
data.x,
data.y,
0.0,
0.0,
));
let stepper_cmds = self
.model
.calc_control_signals(GantryPosition::new(data.x, data.y, 0.0, 0.0));

rosrust::ros_info!(
"X:{}\tY:{}\t",
data.x,
data.y,
);
rosrust::ros_info!("X:{}\tY:{}\t", data.x, data.y,);

/* Move Gantry over weed */
self.write_steppers(stepper_cmds);
Expand All @@ -249,12 +275,9 @@ impl GantryController {
self.actuate_eoat(data.z_buffer);

/* Zero everything */
let stepper_cmds = self.model.calc_control_signals(GantryPosition::new(
0.0,
0.0,
0.0,
0.0,
));
let stepper_cmds = self
.model
.calc_control_signals(GantryPosition::new(0.0, 0.0, 0.0, 0.0));
self.write_steppers(stepper_cmds);
}

Expand All @@ -265,7 +288,13 @@ impl GantryController {

rosrust::ros_info!("CALIBRATING...");

self.write_steppers(stepper_cmds);
self.servo.lock().unwrap().grip();

thread::sleep(Duration::from_secs(2));

self.servo.lock().unwrap().release();

//self.write_steppers(stepper_cmds);
}

pub fn drive_pos_callback(&mut self) {
Expand Down Expand Up @@ -339,12 +368,16 @@ fn main() {
AXIGPIO::new(GANTRY_STEP5_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
)));

let servo = Arc::new(Mutex::new(ServoController::new(
AXITimer::new(SERVO_PWM_ADDR, SIZEOF_AXITIMER_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, stepper4, stepper5, reset,
stepper1, stepper2, stepper3, stepper4, stepper5, servo, reset,
)));

/* Create subscriber */
Expand Down
Loading

0 comments on commit b36cf5d

Please sign in to comment.