Skip to content

Commit

Permalink
Updating Rust code
Browse files Browse the repository at this point in the history
  • Loading branch information
@nwdepatie
nwdepatie committed Apr 2, 2024
1 parent 59ac606 commit e21e5f7
Show file tree
Hide file tree
Showing 5 changed files with 100 additions and 50 deletions.
59 changes: 57 additions & 2 deletions mercury-app/src/hw_ifc/src/chassis_model.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,18 +70,33 @@ impl ChassisModel {
},
};

rosrust::ros_info!("Linear Speed: {} m/s", linear_speed);
rosrust::ros_info!("Angular Speed: {} m/s", angular_speed);
rosrust::ros_info!("Wheel Radius: {} m", self.wheel_radius);

cmds[WheelPositions::LeftWheels].speed = (linear_speed / (2.0 * PI * self.wheel_radius))
+ (angular_speed / (2.0 * self.wheel_radius));

cmds[WheelPositions::RightWheels].speed = (linear_speed / (2.0 * PI * self.wheel_radius))
- (angular_speed / (2.0 * self.wheel_radius));

rosrust::ros_info!(
"Left Vel/Speed: {} rad/s, Right Vel/Speed: {} rad/s",
cmds[WheelPositions::LeftWheels].speed,
cmds[WheelPositions::RightWheels].speed
);

/* Find the maximum absolute speed between the two wheels */
let max_wheel_speed = cmds[WheelPositions::LeftWheels]
.speed
.abs()
.max(cmds[WheelPositions::RightWheels].speed.abs());

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

/* Normalize wheel speeds if exceeding MAX_SPEED */
if max_wheel_speed > self.max_speed {
cmds[WheelPositions::LeftWheels].speed =
Expand All @@ -90,16 +105,56 @@ impl ChassisModel {
(cmds[WheelPositions::RightWheels].speed / max_wheel_speed) * self.max_speed;
}

rosrust::ros_info!(
"Left Vel/Normalized Speed: {} rad/s, Right Vel/Normalized Speed: {} rad/s",
cmds[WheelPositions::LeftWheels].speed,
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 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;
rosrust::ros_info!(
"Left Vel/RPM Scaled: {} (from 0 to 1), Right Vel/RPM Scaled: {} rad/s",
cmds[WheelPositions::LeftWheels].speed,
cmds[WheelPositions::RightWheels].speed
);

/* Convert normalized wheel speeds to PWM duty cycle and direction */
cmds[WheelPositions::LeftWheels].dutycycle =
(cmds[WheelPositions::LeftWheels].speed.abs() * 15.0 / self.max_speed) as u32;
((cmds[WheelPositions::LeftWheels].speed.abs() * (127.0 as f64)) as u32);
cmds[WheelPositions::RightWheels].dutycycle =
(cmds[WheelPositions::RightWheels].speed.abs() * 15.0 / self.max_speed) as u32;
((cmds[WheelPositions::RightWheels].speed.abs() * (127.0 as f64)) as u32);

rosrust::ros_info!(
"Left Vel/RPM As u32: {} / {}, Right Vel/RPM as u32: {} / {}",
cmds[WheelPositions::LeftWheels].speed,
u32::MAX,
cmds[WheelPositions::RightWheels].speed,
u32::MAX
);

cmds[WheelPositions::LeftWheels].direction = cmds[WheelPositions::LeftWheels].speed >= 0.0;
cmds[WheelPositions::RightWheels].direction =
cmds[WheelPositions::RightWheels].speed >= 0.0;

rosrust::ros_info!(
"Left Vel/Duty Cycles: {}, Right Vel/Duty Cycles: {}",
cmds[WheelPositions::LeftWheels].dutycycle,
cmds[WheelPositions::RightWheels].dutycycle
);

rosrust::ros_info!(
"Left Vel/Direction: {}, Right Vel/Direction: {}",
cmds[WheelPositions::LeftWheels].direction,
cmds[WheelPositions::RightWheels].direction
);

cmds
}
}
18 changes: 7 additions & 11 deletions mercury-app/src/hw_ifc/src/drive_sub.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,11 +13,11 @@ use self::regmap::{
use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG};
use zynq::axitimer::{AXITimer, SIZEOF_AXITIMER_REG};

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 */
const BOT_WIDTH: f32 = 10.0; /* meters */
const BOT_LENGTH: f32 = 10.0; /* meters */
const MAX_LINEAR_SPEED: f64 = 100.0; /* meters/second */
const MAX_ANGULAR_SPEED: f64 = 100.0; /* radians/second */
const WHEEL_RADIUS: f64 = 0.095; /* meters */
const BOT_WIDTH: f32 = 0.20; /* meters */
const BOT_LENGTH: f32 = 0.20; /* meters */

type PWMController = Arc<Mutex<AXITimer>>;
type DirectionController = Arc<Mutex<AXIGPIO>>;
Expand Down Expand Up @@ -62,12 +62,8 @@ 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 Down
2 changes: 1 addition & 1 deletion mercury-app/src/hw_ifc/src/gantry_model.rs
View file
Original file line number Diff line number Diff line change
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::ZPrime] - self.current_position[GantryAxes::Z]) as i32;
(target_position[GantryAxes::ZPrime] - self.current_position[GantryAxes::ZPrime]) as i32;

// Update current position
self.current_position = target_position;
Expand Down
31 changes: 15 additions & 16 deletions mercury-app/src/hw_ifc/src/gantry_sub.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,50 +91,49 @@ impl GantryController {

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

if stepper_cmds[GantryAxes::ZPrime].steps != 0 {
self.stepper5
self.stepper1
.lock()
.unwrap()
.set_direction(stepper_cmds[GantryAxes::ZPrime].steps < 0);
self.stepper5
.set_direction(stepper_cmds[GantryAxes::ZPrime].steps > 0);
self.stepper1
.lock()
.unwrap()
.write_step_cmd(i32::abs(stepper_cmds[GantryAxes::ZPrime].steps) as u32);
Expand Down
40 changes: 20 additions & 20 deletions mercury-app/src/hw_ifc/src/regmap.rs
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,22 @@
pub const DRV_DIR_GPIO_ADDR: u32 = 0x7FFF_8000;
pub const DRV_TIMER_FRONT_RIGHT_ADDR: u32 = 0x4000_0000;
pub const DRV_TIMER_FRONT_LEFT_ADDR: u32 = 0x4001_0000;
pub const DRV_TIMER_BACK_RIGHT_ADDR: u32 = 0x4002_0000;
pub const DRV_TIMER_BACK_LEFT_ADDR: u32 = 0x4003_0000;
pub const DRV_DIR_GPIO_ADDR: u32 = 0x4001_8000;
pub const DRV_TIMER_FRONT_RIGHT_ADDR: u32 = 0x4002_0000;
pub const DRV_TIMER_FRONT_LEFT_ADDR: u32 = 0x4003_0000;
pub const DRV_TIMER_BACK_RIGHT_ADDR: u32 = 0x4004_0000;
pub const DRV_TIMER_BACK_LEFT_ADDR: u32 = 0x4005_0000;

pub const GANTRY_DIR1_ADDR: u32 = 0x4004_0000;
pub const GANTRY_DIR2_ADDR: u32 = 0x4005_0000;
pub const GANTRY_DIR3_ADDR: u32 = 0x4006_0000;
pub const GANTRY_DIR4_ADDR: u32 = 0x4007_0000;
pub const GANTRY_DIR5_ADDR: u32 = 0x4008_0000;
pub const GANTRY_RESET_ADDR: u32 = 0x4009_0000;
pub const GANTRY_STEP1_ADDR: u32 = 0x400A_0000;
pub const GANTRY_STEP2_ADDR: u32 = 0x400B_0000;
pub const GANTRY_STEP3_ADDR: u32 = 0x400C_0000;
pub const GANTRY_STEP4_ADDR: u32 = 0x400D_0000;
pub const GANTRY_STEP5_ADDR: u32 = 0x400E_0000;
pub const GANTRY_DIR1_ADDR: u32 = 0x4006_0000;
pub const GANTRY_DIR2_ADDR: u32 = 0x4007_0000;
pub const GANTRY_DIR3_ADDR: u32 = 0x4008_0000;
pub const GANTRY_DIR4_ADDR: u32 = 0x4009_0000;
pub const GANTRY_DIR5_ADDR: u32 = 0x400A_0000;
pub const GANTRY_RESET_ADDR: u32 = 0x400B_0000;
pub const GANTRY_STEP1_ADDR: u32 = 0x400C_0000;
pub const GANTRY_STEP2_ADDR: u32 = 0x400D_0000;
pub const GANTRY_STEP3_ADDR: u32 = 0x400E_0000;
pub const GANTRY_STEP4_ADDR: u32 = 0x400F_0000;
pub const GANTRY_STEP5_ADDR: u32 = 0x4010_0000;

pub const LED_BRAKELIGHT: u32 = 0x400F_0000;
pub const LED_FRONT_LOWER: u32 = 0x4010_0000;
pub const LED_FRONT_UPPER: u32 = 0x4011_0000;
pub const LED_WORKCELL: u32 = 0x4012_0000;
pub const LED_BRAKELIGHT: u32 = 0x4011_0000;
pub const LED_FRONT_LOWER: u32 = 0x4012_0000;
pub const LED_FRONT_UPPER: u32 = 0x4013_0000;
pub const LED_WORKCELL: u32 = 0x4014_0000;

0 comments on commit e21e5f7

Please sign in to comment.