Skip to content

Commit

Permalink
Updating Rust
Browse files Browse the repository at this point in the history
  • Loading branch information
@nwdepatie
nwdepatie committed Mar 31, 2024
1 parent 9b0626c commit 721384b
Show file tree
Hide file tree
Showing 9 changed files with 166 additions and 115 deletions.
16 changes: 4 additions & 12 deletions mercury-app/compose.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,8 @@ services:
roscore:
image: ros:noetic-ros-core
command: /bin/bash -c "roscore"
networks:
ros_network:
ipv4_address: 172.28.1.2
network_mode:
"host"

mercury_app:
build: .
Expand All @@ -14,16 +13,9 @@ services:
tty: true
volumes:
- ./:/home/proj
networks:
ros_network:
ipv4_address: 172.28.1.3
network_mode:
"host"
environment:
- ROS_MASTER_URI=http://172.28.1.2:11311
- ROS_IP=172.28.1.3

networks:
ros_network:
driver: bridge
ipam:
config:
- subnet: 172.28.0.0/16
2 changes: 1 addition & 1 deletion mercury-app/src/hw_ifc/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ include_directories(

add_custom_target(hw_ifc
ALL
COMMAND cargo build -p mercury --release #--target armv7-unknown-linux-gnueabihf
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..."
Expand Down
22 changes: 20 additions & 2 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,6 +6,7 @@ pub enum GantryAxes {
X,
Y,
Z,
Z_Prime,
}

pub struct StepperCtrlCmd {
Expand All @@ -16,6 +17,7 @@ pub struct StepperCtrlCmdGroup {
x: StepperCtrlCmd,
y: StepperCtrlCmd,
z: StepperCtrlCmd,
z_prime: StepperCtrlCmd,
}

impl Index<GantryAxes> for StepperCtrlCmdGroup {
Expand All @@ -26,6 +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,
}
}
}
Expand All @@ -36,6 +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,
}
}
}
Expand All @@ -44,6 +48,7 @@ pub struct GantryPosition {
x: f64,
y: f64,
z: f64,
z_prime: f64,
}

impl Index<GantryAxes> for GantryPosition {
Expand All @@ -54,6 +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,
}
}
}
Expand All @@ -64,13 +70,19 @@ 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,
}
}
}

impl GantryPosition {
pub fn new(x: f64, y: f64, z: f64) -> GantryPosition {
GantryPosition { x: x, y: y, z: z }
pub fn new(x: f64, y: f64, z: f64, z_prime: f64) -> GantryPosition {
GantryPosition {
x: x,
y: y,
z: z,
z_prime: z_prime,
}
}
}

Expand All @@ -85,6 +97,7 @@ impl GantryModel {
x: 0.0,
y: 0.0,
z: 0.0,
z_prime: 0.0,
},
}
}
Expand All @@ -101,6 +114,8 @@ impl GantryModel {
(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;
let z_prime_steps =
(target_position[GantryAxes::Z_Prime] - self.current_position[GantryAxes::Z]) as i32;

// Update current position
self.current_position = target_position;
Expand All @@ -109,6 +124,9 @@ impl GantryModel {
x: StepperCtrlCmd { steps: x_steps },
y: StepperCtrlCmd { steps: y_steps },
z: StepperCtrlCmd { steps: z_steps },
z_prime: StepperCtrlCmd {
steps: z_prime_steps,
},
}
}
}
170 changes: 110 additions & 60 deletions mercury-app/src/hw_ifc/src/gantry_sub.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@ use self::gantry_model::{
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 {
Expand All @@ -19,22 +22,26 @@ 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_RESET_ADDR,
GANTRY_STEP1_ADDR, GANTRY_STEP2_ADDR, GANTRY_STEP3_ADDR, GANTRY_STEP4_ADDR,
};

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

pub struct StepperController {
direction_control: AXIGPIO,
step_control: AXIGPIO,
pub is_running : bool
is_running: bool,
}

impl StepperController {
pub fn new(direction_control: AXIGPIO, step_control: AXIGPIO) -> StepperController {
pub fn new(direction_control: AXIGPIO, mut step_control: AXIGPIO) -> StepperController {
step_control.write_gpio(1, GPIOChannel::GpioChannel1);

StepperController {
direction_control: direction_control,
step_control: step_control,
is_running : false
is_running: false,
}
}

Expand All @@ -44,12 +51,13 @@ impl StepperController {
}

pub fn write_step_cmd(&mut self, num_steps: u32) {
self.step_control.write_gpio(0, GPIOChannel::GpioChannel1);
self.step_control
.write_gpio(num_steps, GPIOChannel::GpioChannel1);
}

pub fn read_done(&mut self) -> u32 {
self.step_control.read_gpio(GPIOChannel::GpioChannel2)
pub fn read_done(&mut self) -> bool {
self.step_control.read_gpio(GPIOChannel::GpioChannel2) != 0
}
}

Expand Down Expand Up @@ -81,71 +89,115 @@ impl GantryController {
}

fn write_steppers(&mut self, stepper_cmds: StepperCtrlCmdGroup) {
self.stepper1
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::X].steps < 0);
self.stepper1
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
self.stepper2
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Y].steps < 0);
self.stepper2
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Y].steps) as u32);
self.stepper3
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Z].steps < 0);
self.stepper3
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Z].steps) as u32);
if stepper_cmds[GantryAxes::X].steps != 0 {
self.stepper1
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::X].steps < 0);
self.stepper1
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
self.stepper4
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::X].steps < 0);
self.stepper4
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::X].steps) as u32);
}
if stepper_cmds[GantryAxes::Y].steps != 0 {
self.stepper2
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Y].steps < 0);
self.stepper2
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::Y].steps) as u32);
}
if stepper_cmds[GantryAxes::Z].steps != 0 {
self.stepper3
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::Z].steps < 0);
self.stepper3
.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);
//}
}

pub fn move_callback(&mut self, data: rosrust_msg::mercury_services::movegantryReq) {
let stepper_cmds = self
.model
.calc_control_signals(GantryPosition::new(data.x, data.y, data.z));

rosrust::ros_info!("X:{}\tY:{}\tZ:{}", data.x, data.y, data.z);
let stepper_cmds = self.model.calc_control_signals(GantryPosition::new(
data.x,
data.y,
data.z,
data.z_prime,
));

rosrust::ros_info!(
"X:{}\tY:{}\tZ:{}\tZ':{}",
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::Z_Prime].steps),
),
) as f64;

self.write_steppers(stepper_cmds);

ros_info!("Waiting {} s", max_steps / STEPPER_SPEED);

thread::sleep(Duration::from_secs_f64(max_steps / STEPPER_SPEED));
}

pub fn calibrate_callback(&mut self) {
let stepper_cmds = self
.model
.calc_control_signals(GantryPosition::new(0.0, 0.0, 0.0));
.calc_control_signals(GantryPosition::new(0.0, 0.0, 0.0, 0.0));

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

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 {
fn cont_write_stepper(stepper: Arc<Mutex<StepperController>>, val: f64) {
let mut stepper_guard = stepper.lock().unwrap();

if !stepper_guard.read_done() {
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;
} else if !stepper_guard.is_running {
ros_info!("Value written: {}", f64::abs(val * 100.0) as u32);
stepper_guard.set_direction(val < 0.0);
stepper_guard.write_step_cmd(f64::abs(val * 100.0) as u32);
stepper_guard.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;
} else {
stepper_guard.is_running = false;
return;
}
}
Expand All @@ -159,12 +211,10 @@ impl GantryController {

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);
Self::cont_write_stepper(self.stepper1.clone(), x);
Self::cont_write_stepper(self.stepper1.clone(), y);
Self::cont_write_stepper(self.stepper1.clone(), z);
Self::cont_write_stepper(self.stepper1.clone(), z_prime);
}
}

Expand Down
11 changes: 3 additions & 8 deletions mercury-app/src/hw_ifc/src/gpio_test.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,17 +2,12 @@
pub mod zynq;
use std::env;
use std::{thread, time};
use zynq::axigpio::{
GPIOChannel::GpioChannel1, GPIOChannel::GpioChannel2, AXIGPIO, SIZEOF_AXIGPIO_REG,
};
use zynq::axigpio::{GPIOChannel::GpioChannel1, AXIGPIO, SIZEOF_AXIGPIO_REG};
pub mod regmap;
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, GANTRY_STEP2_ADDR,
};
use self::regmap::GANTRY_STEP2_ADDR;

fn main() {
let args: Vec<String> = env::args().collect();
let _args: Vec<String> = env::args().collect();

/* Note that this is testing GPIO preconfigured to be an output */
let mut gpio0 = AXIGPIO::new(GANTRY_STEP2_ADDR, SIZEOF_AXIGPIO_REG).unwrap();
Expand Down
Loading

0 comments on commit 721384b

Please sign in to comment.