tweaked turning algorithm

michaelkamprath · Feb 20, 2024 · 5ebc1d7 · 5ebc1d7
1 parent e412215
commit 5ebc1d7
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 18 deletions.
diff --git a/src/driver.rs b/src/driver.rs
@@ -109,6 +109,12 @@ where
                 Point::new(0, 0),
                 Point::new(0, 500),
                 Point::new_with_forward(0, 0, false),
+                Point::new(500, 0),
+                Point::new_with_forward(0, 0, false),
+                Point::new(0, -500),
+                Point::new_with_forward(0, 0, false),
+                Point::new(-500, 0),
+                Point::new_with_forward(0, 0, false),
             ]);
             self.led1.set_low().ok();
         }

diff --git a/src/model/point.rs b/src/model/point.rs
@@ -45,17 +45,18 @@ impl Point {
         let x_diff = -(other.x - self.x);
         let y_diff = other.y - self.y;
         let bearing = (x_diff as f32).atan2(y_diff as f32).to_degrees();
-        if other.forward {
+        let raw_bearing = if other.forward {
             bearing
         } else {
-            let reverse = bearing + 180.0;
-            if reverse > 180.0 {
-                reverse - 360.0
-            } else if reverse < -180.0 {
-                reverse + 360.0
-            } else {
-                reverse
-            }
+            bearing + 180.0
+        };
+
+        if raw_bearing > 180.0 {
+            raw_bearing - 360.0
+        } else if raw_bearing < -180.0 {
+            raw_bearing + 360.0
+        } else {
+            raw_bearing
         }
     }
 }

diff --git a/src/robot.rs b/src/robot.rs
@@ -633,15 +633,23 @@ where
     }
 
     pub fn turn(&mut self, angle_degrees: i32) -> &mut Self {
-        const TURN_MIN_POWER: f32 = 0.35;
+        const TURN_MIN_POWER: f32 = 0.50;
         if angle_degrees.abs() < self.min_turn_angle() as i32 {
             debug!("Turn angle {} too small, not turning", angle_degrees);
             return self;
         }
 
         info!("Robot turn, angle = {}", angle_degrees);
         self.reset_wheel_counters();
-        let direction_str = if angle_degrees > 0 {
+        let turn_degrees = if angle_degrees > 180 {
+            angle_degrees - 360
+        } else if angle_degrees < -180 {
+            angle_degrees + 360
+        } else {
+            angle_degrees
+        };
+
+        let direction_str = if turn_degrees > 0 {
             LEFT_ARROW_STRING
         } else {
             RIGHT_ARROW_STRING
@@ -650,7 +658,7 @@ where
             self.clear_lcd().set_lcd_cursor(0, 0),
             "{} 0 / {}\x03",
             direction_str,
-            angle_degrees,
+            turn_degrees,
         ) {
             error!("Error writing to LCD: {}", error.to_string().as_str());
         }
@@ -662,20 +670,20 @@ where
         // motor A is the left motor, motor B is the right motor
         self.motors
             .set_duty(self.noramlize_duty(1.), self.noramlize_duty(1.));
-        if angle_degrees > 0 {
+        if turn_degrees > 0 {
             self.motors.reverse_a();
             self.motors.forward_b();
         } else {
             self.motors.forward_a();
             self.motors.reverse_b();
         }
 
-        while current_angle.abs() < (angle_degrees.abs() - 12) as f32 {
+        while current_angle.abs() < (turn_degrees.abs() - 12) as f32 {
             current_angle = self.heading_calculator.heading();
 
             if (current_angle - last_adjust_angle).abs() > 5.0 {
                 last_adjust_angle = current_angle;
-                let abs_angle = angle_degrees.abs() as f32;
+                let abs_angle = turn_degrees.abs() as f32;
                 let motor_power = TURN_MIN_POWER
                     + (1.0 - TURN_MIN_POWER) * ((abs_angle - current_angle.abs()) / abs_angle);
                 self.motors.set_duty(
@@ -688,7 +696,7 @@ where
                 "{} {} / {}\x03",
                 direction_str,
                 current_angle as i32,
-                angle_degrees,
+                turn_degrees,
             ) {
                 error!("Error writing to LCD: {}", error.to_string().as_str());
             }
@@ -707,7 +715,7 @@ where
                     "{} {} / {}\x03",
                     direction_str,
                     current_angle as i32,
-                    angle_degrees,
+                    turn_degrees,
                 ) {
                     error!("Error writing to LCD: {}", error.to_string().as_str());
                 }
@@ -719,7 +727,7 @@ where
             "{} {} / {}\x03",
             direction_str,
             current_angle as i32,
-            angle_degrees,
+            turn_degrees,
         ) {
             error!("Error writing to LCD: {}", error.to_string().as_str());
         }