From 5ffcbd4d4f7f542e202daadfb5b749af2bbe581a Mon Sep 17 00:00:00 2001 From: gstrenge Date: Thu, 4 Apr 2024 22:33:40 -0400 Subject: [PATCH] Update comment and change drive calculations to be correct --- mercury-app/src/hw_ifc/src/chassis_model.rs | 8 ++++---- mercury-app/src/hw_ifc/src/drive_sub.rs | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/mercury-app/src/hw_ifc/src/chassis_model.rs b/mercury-app/src/hw_ifc/src/chassis_model.rs index 25bd955..5b66df9 100644 --- a/mercury-app/src/hw_ifc/src/chassis_model.rs +++ b/mercury-app/src/hw_ifc/src/chassis_model.rs @@ -74,11 +74,11 @@ impl ChassisModel { 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::LeftWheels].speed = linear_speed + (angular_speed * (self.width as f64)) / 2.0; + cmds[WheelPositions::RightWheels].speed = linear_speed - (angular_speed * (self.width as f64)) / 2.0; - cmds[WheelPositions::RightWheels].speed = (linear_speed / (2.0 * PI * self.wheel_radius)) - - (angular_speed / (2.0 * self.wheel_radius)); + cmds[WheelPositions::LeftWheels].speed /= self.wheel_radius; + cmds[WheelPositions::RightWheels].speed /= self.wheel_radius; rosrust::ros_info!( "Left Vel/Speed: {} rad/s, Right Vel/Speed: {} rad/s", diff --git a/mercury-app/src/hw_ifc/src/drive_sub.rs b/mercury-app/src/hw_ifc/src/drive_sub.rs index 07bb5b8..e3965ff 100644 --- a/mercury-app/src/hw_ifc/src/drive_sub.rs +++ b/mercury-app/src/hw_ifc/src/drive_sub.rs @@ -13,7 +13,7 @@ use self::regmap::{ use zynq::axigpio::{GPIOChannel, AXIGPIO, SIZEOF_AXIGPIO_REG}; use zynq::axitimer::{AXITimer, SIZEOF_AXITIMER_REG}; -const MAX_LINEAR_SPEED: f64 = 100.0; /* meters/second */ +const MAX_LINEAR_SPEED: f64 = 100.0; /* radians/second */ const MAX_ANGULAR_SPEED: f64 = 100.0; /* radians/second */ const WHEEL_RADIUS: f64 = 0.095; /* meters */ const BOT_WIDTH: f32 = 0.20; /* meters */