Update comment and change drive calculations to be correct

nwdepatie · Apr 5, 2024 · cb0b36c · cb0b36c
1 parent 54ae3f7
commit cb0b36c
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 deletions.
diff --git 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
@@ -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 */