Skip to content

Commit

Permalink
Adding LED functionality and base of EOAT
Browse files Browse the repository at this point in the history
  • Loading branch information
@nwdepatie
nwdepatie committed Apr 1, 2024
1 parent ad73c3f commit f9e92b9
Show file tree
Hide file tree
Showing 11 changed files with 682 additions and 204 deletions.
8 changes: 8 additions & 0 deletions mercury-app/src/hw_ifc/Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,14 @@ path = "src/gantry_sub.rs"
name = "drive_sub"
path = "src/drive_sub.rs"

[[bin]]
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
2 changes: 1 addition & 1 deletion mercury-app/src/hw_ifc/src/drive_sub.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ impl DriveController {
let bot_width = BOT_WIDTH;
let bot_length = BOT_LENGTH;
let max_velocity_meters_per_second = MAX_LINEAR_SPEED;
let max_angular_velocity_rad_per_second = MAX_ANGULAR_SPEED;
let _max_angular_velocity_rad_per_second = MAX_ANGULAR_SPEED;

DriveController {
model: ChassisModel::new(
Expand Down
70 changes: 70 additions & 0 deletions mercury-app/src/hw_ifc/src/eoat_sub.rs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
/* Subscriber for Carrying out Commands for Chassis Drive Motors */
pub mod regmap;
pub mod zynq;
use self::regmap::{LED_BRAKELIGHT};
use std::sync::{Arc, Mutex};
use zynq::axitimer::{AXITimer, SIZEOF_AXITIMER_REG};

pub mod msg {
rosrust::rosmsg_include!(mercury_services / grip, mercury_services / release,);
}

pub struct EOATController {
pwm_ctrl: AXITimer,
}

impl EOATController {
pub fn new(pwm_ctrl: AXITimer) -> Self {
EOATController { pwm_ctrl: pwm_ctrl }
}

fn grip(&mut self) {
rosrust::ros_info!("Gripping EOAT");
//self.pwm_ctrl.start_pwm(100, brightness);
}

fn release(&mut self) {
rosrust::ros_info!("Releasing EOAT");
//self.pwm_ctrl.start_pwm(100, brightness);
}
}

fn main() {
/* Initialize ROS node */
rosrust::init("eoat_sub");

let eoat_ctrl = Arc::new(Mutex::new(EOATController::new(
AXITimer::new(LED_BRAKELIGHT, SIZEOF_AXITIMER_REG).unwrap(),
)));

/* Create service */
let eoat_ctrl_clone = eoat_ctrl.clone();
let _service_grip = rosrust::service::<rosrust_msg::mercury_services::grip, _>(
"/services/eoat/grip",
move |_cmd: rosrust_msg::mercury_services::gripReq| {
let mut eoat_ctrl = eoat_ctrl_clone.lock().unwrap();
eoat_ctrl.grip();

Ok(rosrust_msg::mercury_services::gripRes { status: true })
},
)
.unwrap();

/* Create service */
let eoat_ctrl_clone = eoat_ctrl.clone();
let _service_release = rosrust::service::<rosrust_msg::mercury_services::release, _>(
"/services/eoat/grip",
move |_cmd: rosrust_msg::mercury_services::releaseReq| {
let mut eoat_ctrl = eoat_ctrl_clone.lock().unwrap();
eoat_ctrl.release();

Ok(rosrust_msg::mercury_services::releaseRes { status: true })
},
)
.unwrap();

while rosrust::is_ok() {
/* Spin forever, we only execute things on callbacks from here */
rosrust::spin();
}
}
12 changes: 6 additions & 6 deletions mercury-app/src/hw_ifc/src/gantry_model.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ pub enum GantryAxes {
X,
Y,
Z,
Z_Prime,
ZPrime,
}

pub struct StepperCtrlCmd {
Expand All @@ -28,7 +28,7 @@ impl Index<GantryAxes> for StepperCtrlCmdGroup {
GantryAxes::X => &self.x,
GantryAxes::Y => &self.y,
GantryAxes::Z => &self.z,
GantryAxes::Z_Prime => &self.z_prime,
GantryAxes::ZPrime => &self.z_prime,
}
}
}
Expand All @@ -39,7 +39,7 @@ impl IndexMut<GantryAxes> for StepperCtrlCmdGroup {
GantryAxes::X => &mut self.x,
GantryAxes::Y => &mut self.y,
GantryAxes::Z => &mut self.z,
GantryAxes::Z_Prime => &mut self.z_prime,
GantryAxes::ZPrime => &mut self.z_prime,
}
}
}
Expand All @@ -59,7 +59,7 @@ impl Index<GantryAxes> for GantryPosition {
GantryAxes::X => &self.x,
GantryAxes::Y => &self.y,
GantryAxes::Z => &self.z,
GantryAxes::Z_Prime => &self.z_prime,
GantryAxes::ZPrime => &self.z_prime,
}
}
}
Expand All @@ -70,7 +70,7 @@ impl IndexMut<GantryAxes> for GantryPosition {
GantryAxes::X => &mut self.x,
GantryAxes::Y => &mut self.y,
GantryAxes::Z => &mut self.z,
GantryAxes::Z_Prime => &mut self.z_prime,
GantryAxes::ZPrime => &mut self.z_prime,
}
}
}
Expand Down Expand Up @@ -115,7 +115,7 @@ impl GantryModel {
let z_steps =
(target_position[GantryAxes::Z] - self.current_position[GantryAxes::Z]) as i32;
let z_prime_steps =
(target_position[GantryAxes::Z_Prime] - self.current_position[GantryAxes::Z]) as i32;
(target_position[GantryAxes::ZPrime] - self.current_position[GantryAxes::Z]) as i32;

// Update current position
self.current_position = target_position;
Expand Down
56 changes: 30 additions & 26 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,9 +2,7 @@
use rosrust;
use rosrust::ros_info;
pub mod gantry_model;
use self::gantry_model::{
GantryAxes, GantryModel, GantryPosition, StepperCtrlCmd, StepperCtrlCmdGroup,
};
use self::gantry_model::{GantryAxes, GantryModel, GantryPosition, StepperCtrlCmdGroup};
use std::sync::{Arc, Mutex};
pub mod zynq;
use rosrust_msg::geometry_msgs::Twist;
Expand All @@ -22,8 +20,8 @@ pub mod msg {

pub mod regmap;
use self::regmap::{
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,
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,
};

pub const STEPPER_SPEED: f64 = 770.0; /* Ticks/s */
Expand Down Expand Up @@ -67,6 +65,7 @@ pub struct GantryController {
stepper2: Arc<Mutex<StepperController>>,
stepper3: Arc<Mutex<StepperController>>,
stepper4: Arc<Mutex<StepperController>>,
stepper5: Arc<Mutex<StepperController>>,
reset: Arc<Mutex<AXIGPIO>>,
}

Expand All @@ -76,6 +75,7 @@ impl GantryController {
stepper2: Arc<Mutex<StepperController>>,
stepper3: Arc<Mutex<StepperController>>,
stepper4: Arc<Mutex<StepperController>>,
stepper5: Arc<Mutex<StepperController>>,
reset: Arc<Mutex<AXIGPIO>>,
) -> Self {
GantryController {
Expand All @@ -84,6 +84,7 @@ impl GantryController {
stepper2: stepper2,
stepper3: stepper3,
stepper4: stepper4,
stepper5: stepper5,
reset: reset,
}
}
Expand All @@ -98,47 +99,46 @@ impl GantryController {
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
self.stepper4
self.stepper2
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::X].steps < 0);
self.stepper4
self.stepper2
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
}
if stepper_cmds[GantryAxes::Y].steps != 0 {
self.stepper2
self.stepper3
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Y].steps < 0);
self.stepper2
self.stepper3
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Y].steps) as u32);
}
if stepper_cmds[GantryAxes::Z].steps != 0 {
self.stepper3
self.stepper4
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Z].steps < 0);
self.stepper3
self.stepper4
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Z].steps) as u32);
}

// TODO: Z_Prime
//if stepper_cmds[GantryAxes::Z_Prime].steps != 0 {
// self.stepper4
// .lock()
// .unwrap()
// .set_direction(stepper_cmds[GantryAxes::Z_Prime].steps < 0);
// self.stepper4
// .lock()
// .unwrap()
// .write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Z_Prime].steps) as u32);
//}
if stepper_cmds[GantryAxes::ZPrime].steps != 0 {
self.stepper5
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::ZPrime].steps < 0);
self.stepper5
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::ZPrime].steps) as u32);
}
}

pub fn move_callback(&mut self, data: rosrust_msg::mercury_services::movegantryReq) {
Expand All @@ -164,7 +164,7 @@ impl GantryController {
),
max(
i32::abs(stepper_cmds[GantryAxes::Z].steps),
i32::abs(stepper_cmds[GantryAxes::Z_Prime].steps),
i32::abs(stepper_cmds[GantryAxes::ZPrime].steps),
),
) as f64;

Expand Down Expand Up @@ -203,7 +203,6 @@ impl GantryController {
}

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;
Expand Down Expand Up @@ -242,12 +241,17 @@ fn main() {
AXIGPIO::new(GANTRY_STEP4_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
)));

let stepper5 = Arc::new(Mutex::new(StepperController::new(
AXIGPIO::new(GANTRY_DIR5_ADDR, SIZEOF_AXIGPIO_REG).unwrap(),
AXIGPIO::new(GANTRY_STEP5_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, stepper4, reset,
stepper1, stepper2, stepper3, stepper4, stepper5, reset,
)));

/* Create subscriber */
Expand Down Expand Up @@ -279,7 +283,7 @@ fn main() {
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();
let gantry_ctrl = gantry_ctrl_clone.lock().unwrap();
gantry_ctrl.command_callback(v);
})
.unwrap();
Expand Down
Loading

0 comments on commit f9e92b9

Please sign in to comment.