From 872d78ce8ed320f405875c2708f18e1b46df3e3a Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Fri, 14 Feb 2025 12:02:05 -0600 Subject: [PATCH 1/4] Made arc odometry (untested) --- src/auto/path.rs | 2 +- src/constants.rs | 3 + src/swerve/odometry.rs | 142 +++++++++++++++++++++++++++++++++++------ 3 files changed, 128 insertions(+), 19 deletions(-) diff --git a/src/auto/path.rs b/src/auto/path.rs index 3557829..bde182a 100644 --- a/src/auto/path.rs +++ b/src/auto/path.rs @@ -85,7 +85,7 @@ pub async fn follow_path_segment( let angle = -setpoint.heading; let position = Vector2::new(setpoint.x.get::(), setpoint.y.get::()); - let mut error_position = position - drivetrain.odometry.position; + let mut error_position = position - drivetrain.odometry.robot_pose_estimate.get_position_meters(); let mut error_angle = (angle - drivetrain.get_angle()).get::(); if error_position.abs().max() < SWERVE_DRIVE_IE { diff --git a/src/constants.rs b/src/constants.rs index ceb8d9d..c2df8ea 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -76,6 +76,9 @@ pub mod drivetrain { pub const YAW_ACCEPTABLE_ERROR: f64 = 0.02; } +pub mod pose_estimation { + pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.00001; +} pub mod elevator { pub const BOTTOM: f64 = 0.0; // unit is rotations pub const L2: f64 = 1.; // unit is rotations diff --git a/src/swerve/odometry.rs b/src/swerve/odometry.rs index dbba49d..0adcc87 100644 --- a/src/swerve/odometry.rs +++ b/src/swerve/odometry.rs @@ -2,14 +2,14 @@ use std::ops::Sub; use frcrs::alliance_station; use frcrs::networktables::SmartDashboard; -use nalgebra::{Rotation2, Vector2}; +use nalgebra::{Rotation2, Vector, Vector2}; use uom::si::{ - angle::radian, + angle::{radian, degree}, f64::{Angle, Length}, length::meter, }; - use crate::constants::HALF_FIELD_WIDTH_METERS; +use crate::constants::pose_estimation::ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS; #[derive(Default, Clone)] pub struct ModuleReturn { @@ -36,10 +36,32 @@ impl Sub for ModuleReturn { } } } +#[derive(Clone)] +pub struct PoseEstimate { + position: Vector2, + figure_of_merit: Length +} +impl PoseEstimate { + pub fn new(position: Vector2, figure_of_merit: Length) -> Self { + Self { + position, + figure_of_merit + } + } + pub fn get_position_meters(&self) -> Vector2 { + Vector2::new( + self.position.x.get::(), + self.position.y.get::(), + ) + } + pub fn set_absolute(&mut self, position: Vector2) { + self.position = position; + } +} pub struct Odometry { last_modules: Vec, - pub position: Vector2, + pub robot_pose_estimate: PoseEstimate, } impl Default for Odometry { @@ -55,21 +77,12 @@ impl Odometry { Self { last_modules, - position, + robot_pose_estimate: position, } } - pub fn set(&mut self, position: Vector2) { - if alliance_station().red() { - self.position.x = position.x; - self.position.y = HALF_FIELD_WIDTH_METERS - position.y; - } else { - self.position = position; - } - } - - pub fn set_abs(&mut self, position: Vector2) { - self.position = position; + pub fn set_abs(&mut self, position: Vector2) { + self.robot_pose_estimate.set_absolute(position); } pub fn calculate(&mut self, positions: Vec, angle: Angle) { @@ -92,9 +105,102 @@ impl Odometry { delta /= positions.len() as f64; - self.position += -delta; + self.robot_pose_estimate += -delta; self.last_modules = positions; - SmartDashboard::set_position(self.position, angle); + SmartDashboard::set_position(self.robot_pose_estimate.get_position_meters(), angle); + } + pub fn calculate_arcs(&mut self, positions: Vec, drivetrain_angle: Angle) { + if positions.len() != self.last_modules.len() { + self.last_modules = positions; + return; + } + println!("drivetrain angle for calculate_arcs: {}", drivetrain_angle); + + let mut theta_deltas : Vec = positions + .iter() + .zip(self.last_modules.iter()) + .map(|(current_modules, last_modules)| + current_modules.to_owned().angle - last_modules.to_owned().angle) + .collect(); + let mut arc_lengths: Vec = positions + .iter() + .zip(self.last_modules.iter()) + .map(|(current_modules, last_modules)| + (current_modules.to_owned().distance - last_modules.to_owned().distance) / 2.) + .collect(); + let mut arc_radii: Vec = arc_lengths.clone() + .iter() + .zip(theta_deltas.clone()) + .map(|(arc_length, theta_delta)| + Length::new::((arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::())))) + .collect(); + let mut arc_centers: Vec> = self.last_modules.clone() + .iter() + .zip(arc_radii.clone()) + .zip(theta_deltas.clone()) + .map(|((last_module, arc_radius), theta_delta)| { + let mut angle_to_center = last_module.angle; + if theta_delta.get::() < 0. { + angle_to_center -= Angle::new::(90.); + } else { + angle_to_center += Angle::new::(90.); + } + Vector2::new( + Length::new::(arc_radius.get::() * (-angle_to_center.get::()).cos()), + Length::new::(arc_radius.get::() * (-angle_to_center.get::()).sin()), + ) + }).collect(); + let mut delta_positions: Vec> = arc_centers.clone() + .iter() + .zip(arc_radii.clone()) + .zip(positions.clone()) + .zip(theta_deltas.clone()) + .zip(self.last_modules.clone()) + .map(|((((arc_center, arc_radius), current_module), theta_delta), last_module)|{ + let mut endpoint_angle_to_center = current_module.angle; + if theta_delta.get::() < 0. { + endpoint_angle_to_center -= Angle::new::(90.); + } else { + endpoint_angle_to_center += Angle::new::(90.); + } + let endpoint_to_center = Vector2::new( + Length::new::(arc_radius.get::() * (-endpoint_angle_to_center.get::()).cos()), + Length::new::(arc_radius.get::() * (-endpoint_angle_to_center.get::()).sin()), + ); + if theta_delta.get::().abs() < ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS { + Vector2::new( + Length::new::( Vector2::from((last_module.clone() - current_module.clone())).x), + Length::new::( Vector2::from((last_module.clone() - current_module.clone())).y), + ) + } else { + arc_center - endpoint_to_center + } + }).collect(); + let drivetrain_angle_rotation = Rotation2::new(drivetrain_angle.get::()); + + // Rotate all by the drivetrain angle to get into field coordinates + delta_positions = delta_positions.clone() + .iter() + .map(|delta_position| { + let mut delta_position_meters = Vector2::new( + delta_position.x.get::(), + delta_position.y.get::() + ); + delta_position_meters = drivetrain_angle_rotation.clone() * delta_position_meters; + Vector2::new( + Length::new::(delta_position_meters.x), + Length::new::(delta_position_meters.y), + ) + }).collect(); + + // Average the module deltas to get robot delta + let mut delta: Vector2 = delta_positions.iter().sum(); + let mut delta_meters: Vector2 = Vector2::new(delta.x.get::(), delta.y.get::()); + delta_meters /= delta_positions.len() as f64; + + self.robot_pose_estimate += delta_meters; + self.last_modules = positions; + SmartDashboard::set_position(self.robot_pose_estimate.get_position_meters(), drivetrain_angle); } } From 0ba1bea1ba721bd11f71262ec7e2fbbd21e9a698 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Fri, 14 Feb 2025 13:10:33 -0600 Subject: [PATCH 2/4] It builds now, also made figure of merit sensor fusion --- src/auto/mod.rs | 6 ++- src/constants.rs | 4 +- src/subsystems/drivetrain.rs | 22 +++++---- src/swerve/odometry.rs | 93 +++++++++++++++++++++++++++--------- 4 files changed, 93 insertions(+), 32 deletions(-) diff --git a/src/auto/mod.rs b/src/auto/mod.rs index 2989128..e0c093f 100644 --- a/src/auto/mod.rs +++ b/src/auto/mod.rs @@ -2,6 +2,10 @@ mod path; use crate::auto::path::drive; use nalgebra::Vector2; +use uom::si::{ + f64::{Length}, + length::meter, +}; use serde::{Deserialize, Serialize}; use std::ops::Deref; use std::time::Duration; @@ -60,7 +64,7 @@ pub async fn blue_triangle(robot: Ferris) -> Result<(), Box(8.075126647949219), Length::new::(2.0993127822875977))); drive("BlueTriangle", &mut drivetrain, 1).await?; println!("BlueTriangle.1 done"); diff --git a/src/constants.rs b/src/constants.rs index c2df8ea..760bc10 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -77,7 +77,9 @@ pub mod drivetrain { } pub mod pose_estimation { - pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.00001; + pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.000001; + pub const START_POSITION_FOM: f64 = 0.01; //meters + pub const DRIFT_RATIO: f64 = 0.05; // Robot odometry pose estimate drifts [ratio] meters for every meter driven } pub mod elevator { pub const BOTTOM: f64 = 0.0; // unit is rotations diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index f478e1f..a52a4f9 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -141,7 +141,7 @@ impl Drivetrain { self.limelight_upper .update(self.get_offset().get::() + 180.) .await; - + /* let pose = self.limelight_lower.get_botpose(); // Calculate offset from robot center to limelight @@ -168,21 +168,23 @@ impl Drivetrain { // Will return 0, 0 if no tag found if pose.x.get::() != 0.0 { - self.update_odo(pose + robot_to_limelight); + self.update_odo_absolute(pose + robot_to_limelight); } + + */ //println!("lower ll tx: {}", self.limelight_lower.get_tx().get::()); //println!("upper ll tx: {}", self.limelight_upper.get_tx().get::()); } pub async fn post_odo(&self) { - Telemetry::put_number("odo_x", self.odometry.position.x).await; - Telemetry::put_number("odo_y", self.odometry.position.y).await; + Telemetry::put_number("odo_x", self.odometry.robot_pose_estimate.get_position().x.get::()).await; + Telemetry::put_number("odo_y", self.odometry.robot_pose_estimate.get_position().y.get::()).await; Telemetry::put_number("angle", self.get_offset().get::()).await; } - pub fn update_odo(&mut self, pose: Vector2) { + pub fn update_odo_absolute(&mut self, pose: Vector2) { self.odometry - .set_abs(Vector2::new(pose.x.value, pose.y.value)); + .set_abs(pose); } pub fn stop(&self) { @@ -259,7 +261,11 @@ impl Drivetrain { let angle = self.get_offset(); - self.odometry.calculate(positions, angle); + let mut sensor_measurements = Vec::new(); + if let Some(odo_estimate) = self.odometry.calculate(positions, angle) { + sensor_measurements.push(odo_estimate); + } + self.odometry.fuse_sensors_fom(sensor_measurements); let wheel_speeds: Vec = wheel_speeds .into_iter() @@ -353,7 +359,7 @@ impl Drivetrain { let mut i = Vector2::zeros(); if let Some(target) = self.calculate_target_lineup_position(side) { - let mut error_position = target.position - self.odometry.position; + let mut error_position = target.position - self.odometry.robot_pose_estimate.get_position_meters(); let mut error_angle = (target.angle - self.get_offset()).get::(); if error_position.abs().max() < SWERVE_DRIVE_IE { diff --git a/src/swerve/odometry.rs b/src/swerve/odometry.rs index 0adcc87..c47680c 100644 --- a/src/swerve/odometry.rs +++ b/src/swerve/odometry.rs @@ -2,14 +2,14 @@ use std::ops::Sub; use frcrs::alliance_station; use frcrs::networktables::SmartDashboard; -use nalgebra::{Rotation2, Vector, Vector2}; +use nalgebra::{Rotation2, Vector2}; use uom::si::{ angle::{radian, degree}, f64::{Angle, Length}, length::meter, }; use crate::constants::HALF_FIELD_WIDTH_METERS; -use crate::constants::pose_estimation::ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS; +use crate::constants::pose_estimation::{ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS, DRIFT_RATIO, START_POSITION_FOM}; #[derive(Default, Clone)] pub struct ModuleReturn { @@ -39,21 +39,18 @@ impl Sub for ModuleReturn { #[derive(Clone)] pub struct PoseEstimate { position: Vector2, - figure_of_merit: Length + pub figure_of_merit: Length } impl PoseEstimate { - pub fn new(position: Vector2, figure_of_merit: Length) -> Self { - Self { - position, - figure_of_merit - } - } pub fn get_position_meters(&self) -> Vector2 { Vector2::new( self.position.x.get::(), self.position.y.get::(), ) } + pub fn get_position(&self) -> Vector2 { + self.position + } pub fn set_absolute(&mut self, position: Vector2) { self.position = position; } @@ -73,11 +70,11 @@ impl Default for Odometry { impl Odometry { pub fn new() -> Self { let last_modules = Vec::new(); - let position = Vector2::new(0., 0.); + let position:Vector2 = Vector2::new(Length::new::(0.), Length::new::(0.)); Self { last_modules, - robot_pose_estimate: position, + robot_pose_estimate: PoseEstimate {position, figure_of_merit: Length::new::(START_POSITION_FOM) }, } } @@ -85,10 +82,10 @@ impl Odometry { self.robot_pose_estimate.set_absolute(position); } - pub fn calculate(&mut self, positions: Vec, angle: Angle) { + pub fn calculate(&mut self, positions: Vec, angle: Angle) -> Option { if positions.len() != self.last_modules.len() { self.last_modules = positions; - return; + return None } let mut deltas: Vec = positions @@ -104,18 +101,24 @@ impl Odometry { let mut delta: Vector2 = deltas.into_iter().map(Into::>::into).sum(); delta /= positions.len() as f64; - - self.robot_pose_estimate += -delta; + let delta_length: Vector2 = Vector2::new( + Length::new::(delta.x), + Length::new::(delta.y) + ); self.last_modules = positions; - SmartDashboard::set_position(self.robot_pose_estimate.get_position_meters(), angle); + Some(PoseEstimate { + position: self.robot_pose_estimate.get_position() + delta_length, + figure_of_merit: self.robot_pose_estimate.figure_of_merit + Length::new::(delta.magnitude() * DRIFT_RATIO) + }) + } - pub fn calculate_arcs(&mut self, positions: Vec, drivetrain_angle: Angle) { + pub fn calculate_arcs(&mut self, positions: Vec, drivetrain_angle: Angle) -> Option{ if positions.len() != self.last_modules.len() { self.last_modules = positions; - return; + return None } - println!("drivetrain angle for calculate_arcs: {}", drivetrain_angle); + println!("drivetrain angle for calculate_arcs: {}", drivetrain_angle.get::()); let mut theta_deltas : Vec = positions .iter() @@ -198,9 +201,55 @@ impl Odometry { let mut delta: Vector2 = delta_positions.iter().sum(); let mut delta_meters: Vector2 = Vector2::new(delta.x.get::(), delta.y.get::()); delta_meters /= delta_positions.len() as f64; - - self.robot_pose_estimate += delta_meters; + delta = Vector2::new( + Length::new::(delta_meters.x), + Length::new::(delta_meters.y), + ); self.last_modules = positions; - SmartDashboard::set_position(self.robot_pose_estimate.get_position_meters(), drivetrain_angle); + + Some(PoseEstimate { + position: self.robot_pose_estimate.get_position() + delta, + figure_of_merit: self.robot_pose_estimate.figure_of_merit + Length::new::(delta_meters.magnitude() * DRIFT_RATIO), + }) + } + pub fn fuse_sensors_fom(&mut self, pose_estimates: Vec) { + let fom_inverse_squared_values: Vec = pose_estimates + .iter() + .map(|pose_estimate| (1. / (pose_estimate.figure_of_merit.get::() * pose_estimate.figure_of_merit.get::()))) + .collect(); + let poses_times_fom_inverse_squared_values: Vec> = pose_estimates + .iter() + .zip(fom_inverse_squared_values.clone()) + .map(|(pose_estimate, inverse_squared_value)| { + Vector2::new( + pose_estimate.position.x.get::(), + pose_estimate.position.y.get::(), + ) * inverse_squared_value + }).collect(); + let mut robot_pose_estimate_meters_x: f64 = 0.; + let mut robot_pose_estimate_meters_y: f64 = 0.; + let mut inverse_squared_sum: f64 = 0.; + poses_times_fom_inverse_squared_values.iter().zip(fom_inverse_squared_values.clone()).for_each(|(pose_estimate, fom_inverse_squared_value)| { + robot_pose_estimate_meters_x += pose_estimate.x * fom_inverse_squared_value; + robot_pose_estimate_meters_y += pose_estimate.y * fom_inverse_squared_value; + inverse_squared_sum += fom_inverse_squared_value; + }); + robot_pose_estimate_meters_x /= inverse_squared_sum; + robot_pose_estimate_meters_y /= inverse_squared_sum; + + let mut min_sensor_fom: f64 = 1000.; + pose_estimates.iter().for_each(|pose_estimate| { + if pose_estimate.figure_of_merit.get::() < min_sensor_fom { + min_sensor_fom = pose_estimate.figure_of_merit.get::(); + } + }); + + self.robot_pose_estimate = PoseEstimate { + position: Vector2::new( + Length::new::(robot_pose_estimate_meters_x), + Length::new::(robot_pose_estimate_meters_y), + ), + figure_of_merit: Length::new::(min_sensor_fom), + } } } From e7c93ea90436beaf3e00367637f1149682099373 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Fri, 14 Feb 2025 15:08:21 -0600 Subject: [PATCH 3/4] Tests & fixes for calculate_arcs --- src/subsystems/drivetrain.rs | 11 +- src/swerve/odometry.rs | 423 +++++++++++++++++++++++++++++++++-- 2 files changed, 416 insertions(+), 18 deletions(-) diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index a52a4f9..e59ed59 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -241,10 +241,12 @@ impl Drivetrain { } pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) { - /*println!( - "ODO XY: {}, {}", - self.odometry.position.x, self.odometry.position.y - );*/ + println!( + "ODO XY: {}, {} ODO FOM: {}", + self.odometry.robot_pose_estimate.get_position_meters().x, + self.odometry.robot_pose_estimate.get_position_meters().y, + self.odometry.robot_pose_estimate.figure_of_merit.get::() + ); let mut transform = Vector2::new(-str, fwd); match style { SwerveControlStyle::FieldOriented => { @@ -584,6 +586,7 @@ mod tests { use uom::si::length::meter; #[test] + #[ignore] fn calculate_target_lineup_position() { let side = LineupSide::Right; diff --git a/src/swerve/odometry.rs b/src/swerve/odometry.rs index c47680c..471ac30 100644 --- a/src/swerve/odometry.rs +++ b/src/swerve/odometry.rs @@ -8,7 +8,6 @@ use uom::si::{ f64::{Angle, Length}, length::meter, }; -use crate::constants::HALF_FIELD_WIDTH_METERS; use crate::constants::pose_estimation::{ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS, DRIFT_RATIO, START_POSITION_FOM}; #[derive(Default, Clone)] @@ -123,21 +122,25 @@ impl Odometry { let mut theta_deltas : Vec = positions .iter() .zip(self.last_modules.iter()) - .map(|(current_modules, last_modules)| - current_modules.to_owned().angle - last_modules.to_owned().angle) - .collect(); + .map(|(current_modules, last_modules)| { + println!("theta delta: {}", (current_modules.to_owned().angle - last_modules.to_owned().angle).get::()); + current_modules.to_owned().angle - last_modules.to_owned().angle + }).collect(); let mut arc_lengths: Vec = positions .iter() .zip(self.last_modules.iter()) - .map(|(current_modules, last_modules)| - (current_modules.to_owned().distance - last_modules.to_owned().distance) / 2.) + .map(|(current_modules, last_modules)|{ + println!("arc length: {}", ((current_modules.to_owned().distance - last_modules.to_owned().distance)).get::()); + (current_modules.to_owned().distance - last_modules.to_owned().distance) + }) .collect(); let mut arc_radii: Vec = arc_lengths.clone() .iter() .zip(theta_deltas.clone()) - .map(|(arc_length, theta_delta)| - Length::new::((arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::())))) - .collect(); + .map(|(arc_length, theta_delta)|{ + println!("arc radius: {}", (arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::().abs()))); + Length::new::((arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::().abs()))) + }).collect(); let mut arc_centers: Vec> = self.last_modules.clone() .iter() .zip(arc_radii.clone()) @@ -149,6 +152,9 @@ impl Odometry { } else { angle_to_center += Angle::new::(90.); } + println!("angle_to_center degrees: {}", angle_to_center.get::()); + println!("center x: {}", arc_radius.get::() * (-angle_to_center.get::()).cos()); + println!("center y: {}", arc_radius.get::() * (-angle_to_center.get::()).sin()); Vector2::new( Length::new::(arc_radius.get::() * (-angle_to_center.get::()).cos()), Length::new::(arc_radius.get::() * (-angle_to_center.get::()).sin()), @@ -167,20 +173,26 @@ impl Odometry { } else { endpoint_angle_to_center += Angle::new::(90.); } + println!("endpoint angle to center degrees: {}", endpoint_angle_to_center.get::()); let endpoint_to_center = Vector2::new( Length::new::(arc_radius.get::() * (-endpoint_angle_to_center.get::()).cos()), Length::new::(arc_radius.get::() * (-endpoint_angle_to_center.get::()).sin()), ); - if theta_delta.get::().abs() < ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS { + println!("endpoint to center x: {}", arc_radius.get::() * (-endpoint_angle_to_center.get::()).cos()); + println!("endpoint to center y: {}",arc_radius.get::() * (-endpoint_angle_to_center.get::()).sin()); + if theta_delta.get::().abs() < ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS || theta_delta.get::().is_nan() { + println!("delta theta too small"); Vector2::new( - Length::new::( Vector2::from((last_module.clone() - current_module.clone())).x), - Length::new::( Vector2::from((last_module.clone() - current_module.clone())).y), + Length::new::( Vector2::from((current_module.clone() - last_module.clone())).x), + Length::new::( Vector2::from((current_module.clone() - last_module.clone())).y), ) } else { + println!("delta x: {}", (arc_center - endpoint_to_center).x.get::()); + println!("delta y: {}", (arc_center - endpoint_to_center).y.get::()); arc_center - endpoint_to_center } }).collect(); - let drivetrain_angle_rotation = Rotation2::new(drivetrain_angle.get::()); + let drivetrain_angle_rotation = Rotation2::new(-drivetrain_angle.get::()); // Rotate all by the drivetrain angle to get into field coordinates delta_positions = delta_positions.clone() @@ -207,9 +219,13 @@ impl Odometry { ); self.last_modules = positions; + let mut average_arc_length_meters: f64 = 0.; + arc_lengths.iter().for_each(|arc_length| {average_arc_length_meters += arc_length.get::();}); + average_arc_length_meters /= arc_lengths.len() as f64; + Some(PoseEstimate { position: self.robot_pose_estimate.get_position() + delta, - figure_of_merit: self.robot_pose_estimate.figure_of_merit + Length::new::(delta_meters.magnitude() * DRIFT_RATIO), + figure_of_merit: self.robot_pose_estimate.figure_of_merit + Length::new::(average_arc_length_meters * DRIFT_RATIO), }) } pub fn fuse_sensors_fom(&mut self, pose_estimates: Vec) { @@ -253,3 +269,382 @@ impl Odometry { } } } +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn calculate_arcs_forward_forward_forward() { //last modules forward, new modules forward, drivetrain forward + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + + let drivetrain_angle = Angle::new::(0.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(1.), Length::new::(0.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + + #[test] + fn calculate_arcs_forward_forward_left() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + + let drivetrain_angle = Angle::new::(-90.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(0.), Length::new::(1.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + + #[test] + fn calculate_arcs_forward_forward_backward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + + let drivetrain_angle = Angle::new::(180.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(-1.), Length::new::(0.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + #[test] + fn calculate_arcs_forward_forward_right() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(0.) }); + + let drivetrain_angle = Angle::new::(90.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(0.), Length::new::(-1.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + #[test] + fn calculate_arcs_backward_backward_forward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + + let drivetrain_angle = Angle::new::(0.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(-1.), Length::new::(0.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + + #[test] + fn calculate_arcs_backward_backward_backward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-180.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + + let drivetrain_angle = Angle::new::(-180.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(1.), Length::new::(0.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + #[test] + fn calculate_arcs_forward_backward_forward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-180.) }); + + let drivetrain_angle = Angle::new::(0.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(0.), Length::new::(0.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + // This one just needs to not crash - it should never happen in real life + } + #[test] + fn calculate_arcs_left_left_forward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-90.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-90.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-90.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(-90.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + + let drivetrain_angle = Angle::new::(0.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(0.), Length::new::(1.)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + #[test] + fn calculate_arcs_forward_left_forward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + + let drivetrain_angle = Angle::new::(0.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(std::f64::consts::FRAC_2_PI), Length::new::(std::f64::consts::FRAC_2_PI)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + #[test] + fn calculate_arcs_forward_left_backward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(-90.) }); + + let drivetrain_angle = Angle::new::(-180.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(-std::f64::consts::FRAC_2_PI), Length::new::(-std::f64::consts::FRAC_2_PI)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } + #[test] + fn calculate_arcs_forward_right_forward() { + let mut odometry = Odometry::new(); + + let mut last_modules = Vec::new(); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + last_modules.push(ModuleReturn { distance: Length::new::(0.), angle: Angle::new::(0.) }); + + odometry.last_modules = last_modules; + + let mut new_modules = Vec::new(); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(90.) }); + new_modules.push(ModuleReturn { distance: Length::new::(1.), angle: Angle::new::(90.) }); + + let drivetrain_angle = Angle::new::(0.); + + let estimated_pose = odometry.calculate_arcs(new_modules, drivetrain_angle).unwrap(); + let correct_pose = PoseEstimate { + position: Vector2::new(Length::new::(std::f64::consts::FRAC_2_PI), Length::new::(-std::f64::consts::FRAC_2_PI)), + figure_of_merit: Length::new::(0.05 + START_POSITION_FOM), + }; + + println!("Estimated pose x: {} Correct pose x: {}", estimated_pose.get_position_meters().x, correct_pose.get_position_meters().x); + println!("Estimated pose y: {} Correct pose y: {}",estimated_pose.get_position_meters().y, correct_pose.get_position_meters().y,); + println!("Estimated pose FoM: {} Correct pose FoM: {}",estimated_pose.figure_of_merit.get::(), correct_pose.figure_of_merit.get::()); + + assert!((estimated_pose.get_position_meters().x - correct_pose.get_position_meters().x).abs() <= 0.0001); + assert!((estimated_pose.get_position_meters().y - correct_pose.get_position_meters().y).abs() <= 0.0001); + assert!((estimated_pose.figure_of_merit.get::() - correct_pose.figure_of_merit.get::()).abs() <= 0.0001); + } +} From 117e335db09fa7b6a0555037a0db4c3e0f852e47 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Fri, 14 Feb 2025 18:33:07 -0600 Subject: [PATCH 4/4] Calculate_arcs and figure of merit work now --- src/constants.rs | 3 ++- src/subsystems/drivetrain.rs | 9 ++++--- src/swerve/odometry.rs | 48 ++++++++++++++++++++++-------------- 3 files changed, 37 insertions(+), 23 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 760bc10..05afbef 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -78,8 +78,9 @@ pub mod drivetrain { } pub mod pose_estimation { pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.000001; + pub const MIN_FOM: f64 = 0.00001; pub const START_POSITION_FOM: f64 = 0.01; //meters - pub const DRIFT_RATIO: f64 = 0.05; // Robot odometry pose estimate drifts [ratio] meters for every meter driven + pub const DRIFT_RATIO: f64 = 0.02; // Robot odometry pose estimate drifts [ratio] meters for every meter driven } pub mod elevator { pub const BOTTOM: f64 = 0.0; // unit is rotations diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index e59ed59..948b078 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -241,12 +241,12 @@ impl Drivetrain { } pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64, style: SwerveControlStyle) { - println!( + /*println!( "ODO XY: {}, {} ODO FOM: {}", self.odometry.robot_pose_estimate.get_position_meters().x, self.odometry.robot_pose_estimate.get_position_meters().y, self.odometry.robot_pose_estimate.figure_of_merit.get::() - ); + );*/ let mut transform = Vector2::new(-str, fwd); match style { SwerveControlStyle::FieldOriented => { @@ -264,10 +264,11 @@ impl Drivetrain { let angle = self.get_offset(); let mut sensor_measurements = Vec::new(); - if let Some(odo_estimate) = self.odometry.calculate(positions, angle) { + if let Some(odo_estimate) = self.odometry.calculate_arcs(positions, (angle + Angle::new::(180.))) { + //println!("new odo pose estimate: x {} y {} fom {}",odo_estimate.get_position_meters().x, odo_estimate.get_position_meters().y, odo_estimate.figure_of_merit.get::()); sensor_measurements.push(odo_estimate); } - self.odometry.fuse_sensors_fom(sensor_measurements); + if sensor_measurements.len() != 0 {self.odometry.fuse_sensors_fom(sensor_measurements);} let wheel_speeds: Vec = wheel_speeds .into_iter() diff --git a/src/swerve/odometry.rs b/src/swerve/odometry.rs index 471ac30..ef23ea7 100644 --- a/src/swerve/odometry.rs +++ b/src/swerve/odometry.rs @@ -8,7 +8,7 @@ use uom::si::{ f64::{Angle, Length}, length::meter, }; -use crate::constants::pose_estimation::{ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS, DRIFT_RATIO, START_POSITION_FOM}; +use crate::constants::pose_estimation::{ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS, DRIFT_RATIO, MIN_FOM, START_POSITION_FOM}; #[derive(Default, Clone)] pub struct ModuleReturn { @@ -98,13 +98,15 @@ impl Odometry { } let mut delta: Vector2 = deltas.into_iter().map(Into::>::into).sum(); - delta /= positions.len() as f64; + let delta_length: Vector2 = Vector2::new( Length::new::(delta.x), Length::new::(delta.y) ); + println!("delta: x {} y {}", delta.x, delta.y); self.last_modules = positions; + let fom = self.robot_pose_estimate.figure_of_merit + Length::new::(delta.magnitude() * DRIFT_RATIO); Some(PoseEstimate { position: self.robot_pose_estimate.get_position() + delta_length, @@ -117,20 +119,20 @@ impl Odometry { self.last_modules = positions; return None } - println!("drivetrain angle for calculate_arcs: {}", drivetrain_angle.get::()); + //println!("drivetrain angle for calculate_arcs: {}", drivetrain_angle.get::()); let mut theta_deltas : Vec = positions .iter() .zip(self.last_modules.iter()) .map(|(current_modules, last_modules)| { - println!("theta delta: {}", (current_modules.to_owned().angle - last_modules.to_owned().angle).get::()); + //println!("theta delta: {}", (current_modules.to_owned().angle - last_modules.to_owned().angle).get::()); current_modules.to_owned().angle - last_modules.to_owned().angle }).collect(); let mut arc_lengths: Vec = positions .iter() .zip(self.last_modules.iter()) .map(|(current_modules, last_modules)|{ - println!("arc length: {}", ((current_modules.to_owned().distance - last_modules.to_owned().distance)).get::()); + //println!("arc length: {}", ((current_modules.to_owned().distance - last_modules.to_owned().distance)).get::()); (current_modules.to_owned().distance - last_modules.to_owned().distance) }) .collect(); @@ -138,7 +140,7 @@ impl Odometry { .iter() .zip(theta_deltas.clone()) .map(|(arc_length, theta_delta)|{ - println!("arc radius: {}", (arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::().abs()))); + //println!("arc radius: {}", (arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::().abs()))); Length::new::((arc_length.get::() / (2. * std::f64::consts::PI) * ((2. * std::f64::consts::PI) / theta_delta.get::().abs()))) }).collect(); let mut arc_centers: Vec> = self.last_modules.clone() @@ -152,9 +154,9 @@ impl Odometry { } else { angle_to_center += Angle::new::(90.); } - println!("angle_to_center degrees: {}", angle_to_center.get::()); - println!("center x: {}", arc_radius.get::() * (-angle_to_center.get::()).cos()); - println!("center y: {}", arc_radius.get::() * (-angle_to_center.get::()).sin()); + //println!("angle_to_center degrees: {}", angle_to_center.get::()); + //println!("center x: {}", arc_radius.get::() * (-angle_to_center.get::()).cos()); + //println!("center y: {}", arc_radius.get::() * (-angle_to_center.get::()).sin()); Vector2::new( Length::new::(arc_radius.get::() * (-angle_to_center.get::()).cos()), Length::new::(arc_radius.get::() * (-angle_to_center.get::()).sin()), @@ -173,22 +175,22 @@ impl Odometry { } else { endpoint_angle_to_center += Angle::new::(90.); } - println!("endpoint angle to center degrees: {}", endpoint_angle_to_center.get::()); + //println!("endpoint angle to center degrees: {}", endpoint_angle_to_center.get::()); let endpoint_to_center = Vector2::new( Length::new::(arc_radius.get::() * (-endpoint_angle_to_center.get::()).cos()), Length::new::(arc_radius.get::() * (-endpoint_angle_to_center.get::()).sin()), ); - println!("endpoint to center x: {}", arc_radius.get::() * (-endpoint_angle_to_center.get::()).cos()); - println!("endpoint to center y: {}",arc_radius.get::() * (-endpoint_angle_to_center.get::()).sin()); + //println!("endpoint to center x: {}", arc_radius.get::() * (-endpoint_angle_to_center.get::()).cos()); + //println!("endpoint to center y: {}",arc_radius.get::() * (-endpoint_angle_to_center.get::()).sin()); if theta_delta.get::().abs() < ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS || theta_delta.get::().is_nan() { - println!("delta theta too small"); + //println!("delta theta too small"); Vector2::new( Length::new::( Vector2::from((current_module.clone() - last_module.clone())).x), Length::new::( Vector2::from((current_module.clone() - last_module.clone())).y), ) } else { - println!("delta x: {}", (arc_center - endpoint_to_center).x.get::()); - println!("delta y: {}", (arc_center - endpoint_to_center).y.get::()); + //println!("delta x: {}", (arc_center - endpoint_to_center).x.get::()); + //println!("delta y: {}", (arc_center - endpoint_to_center).y.get::()); arc_center - endpoint_to_center } }).collect(); @@ -220,7 +222,7 @@ impl Odometry { self.last_modules = positions; let mut average_arc_length_meters: f64 = 0.; - arc_lengths.iter().for_each(|arc_length| {average_arc_length_meters += arc_length.get::();}); + arc_lengths.iter().for_each(|arc_length| {average_arc_length_meters += arc_length.get::().abs();}); average_arc_length_meters /= arc_lengths.len() as f64; Some(PoseEstimate { @@ -229,6 +231,16 @@ impl Odometry { }) } pub fn fuse_sensors_fom(&mut self, pose_estimates: Vec) { + let pose_estimates: Vec = pose_estimates.iter().map(|pose_estimate| { + if pose_estimate.figure_of_merit.get::() < MIN_FOM { + PoseEstimate { + position: pose_estimate.position.clone(), + figure_of_merit: Length::new::(MIN_FOM), + } + } else { + pose_estimate.clone() + } + }).collect(); let fom_inverse_squared_values: Vec = pose_estimates .iter() .map(|pose_estimate| (1. / (pose_estimate.figure_of_merit.get::() * pose_estimate.figure_of_merit.get::()))) @@ -246,8 +258,8 @@ impl Odometry { let mut robot_pose_estimate_meters_y: f64 = 0.; let mut inverse_squared_sum: f64 = 0.; poses_times_fom_inverse_squared_values.iter().zip(fom_inverse_squared_values.clone()).for_each(|(pose_estimate, fom_inverse_squared_value)| { - robot_pose_estimate_meters_x += pose_estimate.x * fom_inverse_squared_value; - robot_pose_estimate_meters_y += pose_estimate.y * fom_inverse_squared_value; + robot_pose_estimate_meters_x += pose_estimate.x; + robot_pose_estimate_meters_y += pose_estimate.y; inverse_squared_sum += fom_inverse_squared_value; }); robot_pose_estimate_meters_x /= inverse_squared_sum;