Skip to content

Commit

Permalink
Making gantry work with new EOAT routine
Browse files Browse the repository at this point in the history
  • Loading branch information
@nwdepatie
nwdepatie committed Apr 3, 2024
1 parent c512767 commit 1015ea3
Show file tree
Hide file tree
Showing 3 changed files with 149 additions and 35 deletions.
19 changes: 13 additions & 6 deletions mercury-app/src/hw_ifc/src/gantry_model.rs
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,10 @@
/* 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 enum GantryAxes {
X,
Y,
Expand Down Expand Up @@ -44,6 +47,7 @@ impl IndexMut<GantryAxes> for StepperCtrlCmdGroup {
}
}

#[derive(Debug, Clone, Copy)]
pub struct GantryPosition {
x: f64,
y: f64,
Expand Down Expand Up @@ -106,16 +110,19 @@ impl GantryModel {
self.current_position = pos;
}

pub fn get_position(&self) -> GantryPosition {
self.current_position
}

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;
((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]) as i32;
((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]) as i32;
((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]) as i32;
((target_position[GantryAxes::ZPrime] - self.current_position[GantryAxes::ZPrime]) / MM_PER_STEP) as i32;

// Update current position
self.current_position = target_position;
Expand Down
163 changes: 135 additions & 28 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,19 +2,19 @@
use rosrust;
use rosrust::ros_info;
pub mod gantry_model;
use self::gantry_model::{GantryAxes, GantryModel, GantryPosition, StepperCtrlCmdGroup};
use self::gantry_model::{GantryAxes, GantryModel, GantryPosition, StepperCtrlCmdGroup, MM_PER_STEP, GANTRY_X_MAX_MM, GANTRY_Y_MAX_MM};
use std::sync::{Arc, Mutex};
pub mod zynq;
use rosrust_msg::geometry_msgs::Twist;
use std::cmp::max;
use std::thread;
use std::time::Duration;
use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};

pub mod msg {
rosrust::rosmsg_include!(
mercury_services / movegantry,
mercury_services / calibrategantry
mercury_services / calibrategantry,
mercury_services / rtdgantry,
);
}

Expand All @@ -24,8 +24,6 @@ use self::regmap::{
GANTRY_STEP1_ADDR, GANTRY_STEP2_ADDR, GANTRY_STEP3_ADDR, GANTRY_STEP4_ADDR, GANTRY_STEP5_ADDR,
};

pub const STEPPER_SPEED: f64 = 770.0; /* Ticks/s */

pub struct StepperController {
direction_control: AXIGPIO,
step_control: AXIGPIO,
Expand Down Expand Up @@ -94,15 +92,15 @@ impl GantryController {
self.stepper2
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::X].steps > 0);
.set_direction(stepper_cmds[GantryAxes::X].steps < 0); /* true = robot forward */
self.stepper2
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
self.stepper3
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::X].steps > 0);
.set_direction(stepper_cmds[GantryAxes::X].steps < 0);
self.stepper3
.lock()
.unwrap()
Expand All @@ -112,7 +110,7 @@ impl GantryController {
self.stepper4
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Y].steps > 0);
.set_direction(stepper_cmds[GantryAxes::Y].steps < 0); /* true = robot left */
self.stepper4
.lock()
.unwrap()
Expand All @@ -122,7 +120,7 @@ impl GantryController {
self.stepper5
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Z].steps > 0);
.set_direction(stepper_cmds[GantryAxes::Z].steps > 0); /* false = robot up */
self.stepper5
.lock()
.unwrap()
Expand All @@ -132,46 +130,132 @@ impl GantryController {
self.stepper1
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::ZPrime].steps > 0);
.set_direction(stepper_cmds[GantryAxes::ZPrime].steps > 0); /* false = robot up */
self.stepper1
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::ZPrime].steps) as u32);
}

/* Wait for all commands to finish */
while !self.stepper1.lock().unwrap().read_done() {
thread::sleep(Duration::from_secs(1));
}
while !self.stepper2.lock().unwrap().read_done() {
thread::sleep(Duration::from_secs(1));
}
while !self.stepper3.lock().unwrap().read_done() {
thread::sleep(Duration::from_secs(1));
}
while !self.stepper4.lock().unwrap().read_done() {
thread::sleep(Duration::from_secs(1));
}
while !self.stepper5.lock().unwrap().read_done() {
thread::sleep(Duration::from_secs(1));
}
}

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

/* Lower z1 axis by 72mm */
let mut lower_z1 = current_pos.clone();
lower_z1[GantryAxes::ZPrime] += 72.0;
let stepper_cmds = self.model.calc_control_signals(lower_z1);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Lower z axis by *140mm + configurable buffer for distance to ground */
let mut lower_z = current_pos.clone();
lower_z[GantryAxes::Z] += (60.0 + z_tune);
let stepper_cmds = self.model.calc_control_signals(lower_z);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Simulataneously lower z axis and raise z1 axis by 45mm */
let mut lower_z_z1 = current_pos.clone();
lower_z_z1[GantryAxes::Z] += 45.0;
lower_z_z1[GantryAxes::ZPrime] -= 45.0;
let stepper_cmds = self.model.calc_control_signals(lower_z_z1);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Fully retract z1 27mm up */
let mut raise_z1 = current_pos.clone();
raise_z1[GantryAxes::ZPrime] -= 27.0;
let stepper_cmds = self.model.calc_control_signals(raise_z1);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Simultaneously rotate servo for X seconds and lower z by 16mm */
let mut lower_z = current_pos.clone();
lower_z[GantryAxes::Z] += 16.0;
let stepper_cmds = self.model.calc_control_signals(lower_z);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Fully retract z axis (should b 156mm) */
let mut raise_z = current_pos.clone();
raise_z[GantryAxes::Z] -= (121.0 + z_tune);
let stepper_cmds = self.model.calc_control_signals(raise_z);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Optional, move over trash */

/* Open servo */

/* Lower z1 by 72mm */
let mut lower_z1 = current_pos.clone();
lower_z1[GantryAxes::ZPrime] += 72.0;
let stepper_cmds = self.model.calc_control_signals(lower_z1);
self.write_steppers(stepper_cmds);
let current_pos = self.model.get_position();

/* Raise z1 by 72mm */
let mut lower_z1 = current_pos.clone();
lower_z1[GantryAxes::ZPrime] -= 72.0;
let stepper_cmds = self.model.calc_control_signals(lower_z1);
self.write_steppers(stepper_cmds);
}

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,
));
self.write_steppers(stepper_cmds);

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

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

let max_steps = max(
max(
i32::abs(stepper_cmds[GantryAxes::X].steps),
i32::abs(stepper_cmds[GantryAxes::Y].steps),
),
max(
i32::abs(stepper_cmds[GantryAxes::Z].steps),
i32::abs(stepper_cmds[GantryAxes::ZPrime].steps),
),
) as f64;

/* Move Gantry over weed */
self.write_steppers(stepper_cmds);

ros_info!("Waiting {} s", max_steps / STEPPER_SPEED);
/* Actuate End Effector */
self.actuate_eoat(data.z_buffer);

thread::sleep(Duration::from_secs_f64(max_steps / STEPPER_SPEED));
/* Zero everything */
let stepper_cmds = self.model.calc_control_signals(GantryPosition::new(
0.0,
0.0,
0.0,
0.0,
));
self.write_steppers(stepper_cmds);
}

pub fn calibrate_callback(&mut self) {
Expand All @@ -184,6 +268,16 @@ impl GantryController {
self.write_steppers(stepper_cmds);
}

pub fn drive_pos_callback(&mut self) {
let stepper_cmds = self.model.calc_control_signals(GantryPosition::new(
GANTRY_X_MAX_MM,
GANTRY_Y_MAX_MM,
0.0,
0.0,
));
self.write_steppers(stepper_cmds);
}

fn cont_write_stepper(stepper: Arc<Mutex<StepperController>>, val: f64) {
let mut stepper_guard = stepper.lock().unwrap();

Expand Down Expand Up @@ -279,6 +373,19 @@ fn main() {
)
.unwrap();

/* Create service (ready to drive)*/
let gantry_ctrl_clone = gantry_ctrl.clone();
let _service_rtd = rosrust::service::<rosrust_msg::mercury_services::rtdgantry, _>(
"/services/gantry/readytodrive",
move |_| {
let mut gantry_ctrl = gantry_ctrl_clone.lock().unwrap();
gantry_ctrl.drive_pos_callback();

Ok(rosrust_msg::mercury_services::rtdgantryRes { status: true })
},
)
.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| {
Expand Down
2 changes: 1 addition & 1 deletion mercury-app/src/mercury_services
Submodule mercury_services updated 3 files
+1 −0 CMakeLists.txt
+1 −2 srv/movegantry.srv
+2 −0 srv/rtdgantry.srv

0 comments on commit 1015ea3

Please sign in to comment.