From c253c4b862b96c8b9168e24d5fff92c1bbf25b05 Mon Sep 17 00:00:00 2001 From: Michael Kamprath Date: Sun, 18 Feb 2024 14:28:22 -0800 Subject: [PATCH] added ability to traverse a segment in reverse --- src/driver.rs | 12 +++--------- src/model/heading.rs | 6 +----- src/model/point.rs | 28 +++++++++++++++++++++++++--- src/robot.rs | 24 +++++++++++++++++------- 4 files changed, 46 insertions(+), 24 deletions(-) diff --git a/src/driver.rs b/src/driver.rs index 985cd55..d972e45 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -76,10 +76,8 @@ where self.led1.set_high().ok(); self.trace_path(&[ Point::new(0, 0), - Point::new(500, 0), - Point::new(500, 500), Point::new(0, 500), - Point::new(0, 0), + Point::new_with_forward(0, 0, false), ]); self.led1.set_low().ok(); } @@ -109,11 +107,7 @@ where let distance = cur_point.distance_to(next_point); let bearing = cur_point.absolute_bearing(next_point); let bearing_diff = bearing - cur_bearing; - - debug!( - "cur_point: {:?}, next_point: {:?}, distance: {}, bearing: {}, bearing_diff: {}", - cur_point, next_point, distance, bearing, bearing_diff - ); + debug!("driving to next way point\n cur_point: {:?}, next_point: {:?}\n distance: {}, bearing {}, forward: {}", cur_point, next_point, distance, bearing, next_point.forward()); // turn to the correct bearing if bearing_diff.abs() > self.robot.min_turn_angle() { @@ -121,7 +115,7 @@ where } // drive the distance - self.robot.straight(distance as u32); + self.robot.straight(distance as u32, next_point.forward()); // update the current point and bearing cur_point = *next_point; diff --git a/src/model/heading.rs b/src/model/heading.rs index 9069cfd..c0213a5 100644 --- a/src/model/heading.rs +++ b/src/model/heading.rs @@ -18,7 +18,7 @@ use mpu6050::Mpu6050; // [-2241,-2240] --> [-1,15] [1803,1804] --> [-10,6] [1573,1574] --> [16371,16394] [98,99] --> [-2,1] [44,45] --> [0,5] [19,20] --> [0,4] // [-2239,-2238] --> [-3,16] [1804,1804] --> [0,5] [1575,1576] --> [16378,16395] [98,99] --> [-1,2] [43,44] --> [-2,1] [19,20] --> [-1,2] -const HEADING_UPDATE_INTERVAL_MS: u32 = 2; +const HEADING_UPDATE_INTERVAL_MS: u32 = 1; pub struct HeadingCalculator { heading: f32, @@ -104,10 +104,6 @@ where } else if self.heading < -180.0 { self.heading += 360.0; } - // debug!( - // "{}: delta_rads: {}, heading: {}", - // self.gyro, delta_rads, self.heading - // ); } Err(error) => { error!("Error reading gyro: {}", error); diff --git a/src/model/point.rs b/src/model/point.rs index 18cea72..fc54668 100644 --- a/src/model/point.rs +++ b/src/model/point.rs @@ -6,11 +6,16 @@ use micromath::F32Ext; pub struct Point { x: i32, y: i32, + forward: bool, } impl Point { pub fn new(x: i32, y: i32) -> Self { - Self { x, y } + Point::new_with_forward(x, y, true) + } + + pub fn new_with_forward(x: i32, y: i32, forward: bool) -> Self { + Self { x, y, forward } } pub fn x(&self) -> i32 { @@ -21,6 +26,10 @@ impl Point { self.y } + pub fn forward(&self) -> bool { + self.forward + } + /// The distance from this point to the other point pub fn distance_to(&self, other: &Point) -> f32 { let x_diff = self.x - other.x; @@ -30,11 +39,24 @@ impl Point { /// The absolute bearing from this point to the other point, in degrees. 0 degrees pointing postively in the y axis, and /// the right hand rule is used to determine the angle. 90 degrees is pointing negatively in the x axis, 180 degrees - /// radians is pointing negatively in the y axis, and -90 degrees is pointing positively in the x axis + /// radians is pointing negatively in the y axis, and -90 degrees is pointing positively in the x axis. + /// Takes into account the forward direction of the robot. pub fn absolute_bearing(&self, other: &Point) -> f32 { let x_diff = -(other.x - self.x); let y_diff = other.y - self.y; - (x_diff as f32).atan2(y_diff as f32).to_degrees() + let bearing = (x_diff as f32).atan2(y_diff as f32).to_degrees(); + 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 + } + } } } diff --git a/src/robot.rs b/src/robot.rs index abb3825..833ac78 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -294,11 +294,17 @@ where self } - pub fn straight(&mut self, distance_mm: u32) -> &mut Self { + pub fn straight(&mut self, distance_mm: u32, forward: bool) -> &mut Self { + let direction_arrow = if forward { + UP_ARROW_STRING + } else { + DOWN_ARROW_STRING + }; + if let Err(error) = core::write!( self.clear_lcd().set_lcd_cursor(0, 0), "{} 0 / {} mm", - UP_ARROW_STRING, + direction_arrow, distance_mm, ) { error!("Error writing to LCD: {}", error.to_string().as_str()); @@ -338,7 +344,11 @@ where 0., )); let mut update_count: u32 = 0; - self.motors.forward(); + if forward { + self.motors.forward(); + } else { + self.motors.reverse(); + } while traveled_ticks < expected_ticks { cortex_m::interrupt::free(|cs| { @@ -353,15 +363,15 @@ where let heading = self.heading_calculator.heading(); let control_signal = controller.update(heading, last_update_millis); - let mut cs_indicator: &str = UP_ARROW_STRING; + let mut cs_indicator: &str = direction_arrow; // positive control signal means turn left, a negative control signal means turn right let mut left_power: f32 = 1.; let mut right_power: f32 = 1.; - if control_signal > 0. { + if (forward && control_signal > 0.) || (!forward && control_signal < 0.) { left_power = 1. - control_signal; right_power = 1.0; cs_indicator = LEFT_ARROW_STRING; - } else if control_signal < 0. { + } else if (forward && control_signal < 0.) || (!forward && control_signal > 0.) { left_power = 1.0; right_power = 1. + control_signal; cs_indicator = RIGHT_ARROW_STRING; @@ -385,7 +395,7 @@ where if let Err(error) = core::write!( self.set_lcd_cursor(0, 0), "{} {} / {} mm", - UP_ARROW_STRING, + direction_arrow, (traveled_ticks as f32 * MM_PER_WHEEL_TICK) as i32, distance_mm, ) {