diff --git a/Cargo.toml b/Cargo.toml index 9f9fe55..e65a6c4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,8 +13,9 @@ icon = "cool-x" compress = true [dependencies] -vexide = "0.7.0" +vexide = { version = "0.7.0" } libm = { version = "0.2", default-features = false } uom = { version = "0.36.0", default-features = false, features = ["f64", "si"] } embassy-sync = "0.7.0" spin = { version = "0.9", default-features = false, features = ["once", "mutex", "spin_mutex"] } +heapless = "0.8" diff --git a/ascii.md b/ascii.md new file mode 100644 index 0000000..04ef7b3 --- /dev/null +++ b/ascii.md @@ -0,0 +1,9 @@ + $$$$$$\ $$\ $$\ $$\ +$$ __$$\ $$ | \__|$$ | +$$ / \__| $$$$$$\ $$$$$$\ $$\ $$\ $$ | $$\ $$$$$$$\ +$$ |$$$$\ $$ __$$\ \____$$\\$$\ $$ |$$ | $$ |$$ __$$\ +$$ |\_$$ |$$ | \__|$$$$$$$ |\$$\$$ / $$ | $$ |$$ | $$ | +$$ | $$ |$$ | $$ __$$ | \$$$ / $$ | $$ |$$ | $$ | +\$$$$$$ |$$ | \$$$$$$$ | \$ / $$$$$$$$\ $$ |$$$$$$$ | + \______/ \__| \_______| \_/ \________|\__|\_______/ +############################################################### \ No newline at end of file diff --git a/src/GravLib/actuator/mod.rs b/src/GravLib/actuator/mod.rs index 56bf82c..fff4293 100644 --- a/src/GravLib/actuator/mod.rs +++ b/src/GravLib/actuator/mod.rs @@ -2,6 +2,9 @@ pub mod motor_group; // pub mod smartmotor; // TODO - FIX MOTORGROUP +// pub use smartmotor::SmartMotor; +// pub mod smartmotor; + // pub use smartmotor::SmartMotor; pub use motor_group::MotorGroup; \ No newline at end of file diff --git a/src/GravLib/actuator/motor_group.rs b/src/GravLib/actuator/motor_group.rs index 3bf189d..0a43ad8 100644 --- a/src/GravLib/actuator/motor_group.rs +++ b/src/GravLib/actuator/motor_group.rs @@ -11,19 +11,15 @@ use vexide::task; use libm::roundf; pub struct MotorGroup { - inner: &'static Mutex, -} - -struct Inner { motors: Vec, } -impl Inner{ - fn new(motors: Vec) -> Self { +impl MotorGroup { + pub fn new(motors: Vec) -> Self { Self { motors } } - fn move_voltage(&mut self, voltage: f64) { + pub fn move_voltage(&mut self, voltage: f64) { for motor in &mut self.motors { let _ = motor.set_voltage(voltage); } @@ -79,127 +75,4 @@ impl Inner{ let _ = motor.brake(mode); } } -} - - -impl MotorGroup { - pub fn new(motors: Vec) -> Self { - let boxed = Box::new(Mutex::new(Inner::new(motors))); - let static_mutex = Box::leak(boxed); // leak the Box to get a static reference - - let handle: MotorGroup = MotorGroup {inner: static_mutex}; - // task::spawn(Self::tracking_task(handle.inner)).detach(); // TODO - optional, useable in chasiss, spawn background tracking task - - handle - } - - pub fn move_voltage(&self, voltage: f64) { - let mut guard = self.inner.lock(); - guard.move_voltage(voltage); - } - - pub fn move_velocity(&self, velocity_percentage: f64) { - let mut guard = self.inner.lock(); - guard.move_velocity(velocity_percentage); - } - - pub fn voltage(&self) -> f64 { - let guard = self.inner.lock(); - guard.voltage() - } - - pub fn position(&self) -> f64 { - let guard = self.inner.lock(); - guard.position() - } - - pub fn brake(&self, mode: BrakeMode) { - let mut guard = self.inner.lock(); - guard.brake(mode); - } -} - - - - - - - - - - - - -// _________________________________________ // - - - - - -// pub struct motorGroup { -// motors: Vec -// } - - -// impl motorGroup { -// pub fn new(motors: Vec) -> Self { -// Self { motors } -// } - -// pub fn set_voltage(&mut self, voltage: f64) { -// for motor in self.motors.iter_mut() { -// let _ = motor.set_voltage(voltage); -// } -// } - -// pub fn voltage(&self) -> f64 { -// let mut total = 0.0; - -// for motor in &self.motors { -// if let Ok(voltage) = motor.voltage() { -// total += voltage; -// } -// } -// total / self.motors.len() as f64 -// } - - -// pub fn position(&self) -> f64 { -// let mut total = 0.0; -// for motor in &self.motors { -// if let Ok(angle) = motor.position() { -// total += angle.as_radians(); -// } -// } -// total -// } - -// // @dev_note: set_velocity method is built in PID by VEXIDE devs. -// pub fn set_velocity(&mut self, velocity_percentage: f64) { -// // Calculate velocity as percentage of max velocity - -// for motor in self.motors.iter_mut() { -// let gearset = motor.gearset().unwrap(); - -// let max_rpm = match gearset { -// Gearset::Red => 100, -// Gearset::Green => 200, -// Gearset::Blue => 600, -// }; - -// // Convert percentages to rpm -// let velocity_raw = -// (velocity_percentage as f32 / 100.0) -// * (max_rpm as f32); - -// let velocity = roundf(velocity_raw) as i32; -// let _ = motor.set_velocity(velocity); -// } -// } - -// pub fn brake(&mut self, mode: BrakeMode) { -// for motor in self.motors.iter_mut() { -// let _ = motor.brake(mode); -// } -// } -// } \ No newline at end of file +} \ No newline at end of file diff --git a/src/GravLib/actuator/smartmotor.rs b/src/GravLib/actuator/smartmotor.rs deleted file mode 100644 index a081964..0000000 --- a/src/GravLib/actuator/smartmotor.rs +++ /dev/null @@ -1,113 +0,0 @@ -use crate::GravLib::PID; -use crate::GravLib::actuator::motor_group::MotorGroup; - -use vexide::devices::smart::rotation::RotationSensor; -use spin::Mutex; - -/** - * TODO List: - * [] - Test movePID system - * [x] - Implement mutex for SmartMotor readings. - */ - -pub struct SmartMotor { - inner: &'static Mutex -} - -struct Inner { - actuator: MotorGroup, - sensor: RotationSensor, - controller: PID -} - -impl Inner { - fn new (actuator: MotorGroup, sensor: RotationSensor, controller: PID) -> Self { - let controller = PID::new(0.0, 0.0, 0.0, 0.0); - Self { - controller, - sensor, - actuator - } - } - - fn movePID(&mut self, target: f64, timeout: f64, acceptable_range: f64, asyncro: bool, debug: bool) -> i32{ - if asyncro { - // Start thread - // recall method with same parameters - vexide::task::spawn(async move { - self.movePID(target, timeout, acceptable_range, false, debug); - }); - return 2; // indicate async operation - } - - let start_time = vexide::time::now(); - - while (true) { - let mut current_pos = self.sensor.position(); - let mut error = target - current_pos; - - if error.abs() < acceptable_range { - break; // exit condition -> target reached - } - - if vexide::time::now() - start_time > timeout { - break; // exit condition -> timeout reached - } - - let mut control_signal = self.controller.update(error); - - self.actuator.move_voltage(control_signal); - - if debug { - println!("Current Position: {}, Target: {}, Error: {}, Control Signal: {}", - current_pos, target, error, control_signal); - } - - vexide::task::sleep(vexide::time::Duration::from_millis(10)); // Sleep to prevent busy-waiting - } - self.actuator.brake(); - self.controller.reset(); // @dev_note: CRITICAL!! Resets PID Integral - - return if (target - self.get_rotation()).abs() < 0.01 { 1 } else { 0 }; - } - - fn reset(&mut self) { - self.sensor.reset_position(); - } - - fn get_rotation(&self) -> f64 { - self.sensor.position() - } -} - -impl SmartMotor { - pub fn new(actuator: MotorGroup, sensor: RotationSensor, controller: PID) -> Self { - let boxed = Box::new(Mutex::new(Inner::new(actuator, sensor, controller))); - - let static_mutex = Box::leak(boxed); // leak the Box to get a static reference - - let handle: SmartMotor = SmartMotor { inner: static_mutex }; - - handle - } - - pub fn movePID(&self, target: f64, timeout: f64, acceptable_range: f64, asyncro: bool, debug: bool) -> i32 { - let mut guard = self.inner.lock(); - guard.movePID(target, timeout, acceptable_range, asyncro, debug) - } - - pub fn reset(&self) { - let mut guard = self.inner.lock(); - guard.reset(); - } - - pub fn get_rotation(&self) -> f64 { - let guard = self.inner.lock(); - guard.get_rotation() - } - - pub fn get_rotation(&self) -> f64 { - let guard = self.inner.lock(); - guard.get_rotation() - } -} \ No newline at end of file diff --git a/src/GravLib/drivebase/chassis.rs b/src/GravLib/drivebase/chassis.rs deleted file mode 100644 index 412a8ff..0000000 --- a/src/GravLib/drivebase/chassis.rs +++ /dev/null @@ -1,21 +0,0 @@ -use crate::GravLib::actuator::MotorGroup; -use super::Drivetrain; - -/// top‐level Chassis type. -/// Holds a `Drivetrain` and exposes all behaviors from -/// the `drive` and `motions` sub‐modules. -pub struct Chassis { - pub drivetrain: Drivetrain, -} - -impl Chassis { - pub fn new( - left: MotorGroup, - right: MotorGroup, - track_width: f64, - wheel_diameter: f64, - ) -> Self { - let drivetrain = Drivetrain::new(left, right, track_width, wheel_diameter); - Chassis { drivetrain } - } -} diff --git a/src/GravLib/drivebase/driver/mod.rs b/src/GravLib/drivebase/driver/mod.rs deleted file mode 100644 index ec63ae0..0000000 --- a/src/GravLib/drivebase/driver/mod.rs +++ /dev/null @@ -1,5 +0,0 @@ -//! Raw drive‐mode implementations live here: -mod tank; -mod split_arcade; -mod single_arcade; - diff --git a/src/GravLib/drivebase/driver/single_arcade.rs b/src/GravLib/drivebase/driver/single_arcade.rs deleted file mode 100644 index f1159ac..0000000 --- a/src/GravLib/drivebase/driver/single_arcade.rs +++ /dev/null @@ -1,17 +0,0 @@ - -use crate::GravLib::drivebase::Chassis; -use vexide::devices::controller::Controller; - -impl Chassis { - pub fn single_arcade_drive(&mut self, controller: &Controller) { - let s = controller.state().unwrap_or_default(); - let mut speed = s.left_stick.y_raw() as f64 * (12.0 / 127.0); - let turn = s.right_stick.x_raw() as f64 * (12.0 / 127.0); - - // Adjust speed based on the turn input - speed += turn; - - self.drivetrain.left_motors.move_voltage(speed.into()); - self.drivetrain.right_motors.move_voltage(speed.into()); - } -} diff --git a/src/GravLib/drivebase/driver/split_arcade.rs b/src/GravLib/drivebase/driver/split_arcade.rs deleted file mode 100644 index e478f45..0000000 --- a/src/GravLib/drivebase/driver/split_arcade.rs +++ /dev/null @@ -1,18 +0,0 @@ -use crate::GravLib::drivebase::Chassis; -use vexide::devices::controller::Controller; - -impl Chassis { - pub fn split_arcade_drive(&mut self, controller: &Controller) { - let s = controller.state().unwrap_or_default(); - let mut l = s.left_stick.y_raw() as f64 * (12.0 / 127.0); - let mut r = s.right_stick.y_raw() as f64 * (12.0 / 127.0); - let turn = s.right_stick.x_raw() as f64 * (12.0 / 127.0); - - // Adjust left and right speeds based on the turn input - l += turn; - r -= turn; - - self.drivetrain.left_motors.move_voltage(l.into()); - self.drivetrain.right_motors.move_voltage(r.into()); - } -} diff --git a/src/GravLib/drivebase/driver/tank.rs b/src/GravLib/drivebase/driver/tank.rs deleted file mode 100644 index cf60ca6..0000000 --- a/src/GravLib/drivebase/driver/tank.rs +++ /dev/null @@ -1,12 +0,0 @@ -use crate::GravLib::drivebase::Chassis; -use vexide::devices::controller::Controller; - -impl Chassis { - pub fn tank_drive(&mut self, controller: &Controller) { - let s = controller.state().unwrap_or_default(); - let mut l = s.left_stick.y_raw() as f64 * (12.0/127.0); - let mut r = s.right_stick.y_raw() as f64 * (12.0/127.0); - self.drivetrain.left_motors .move_voltage(l.into()); - self.drivetrain.right_motors.move_voltage(r.into()); - } -} diff --git a/src/GravLib/drivebase/drivetrain.rs b/src/GravLib/drivebase/drivetrain.rs deleted file mode 100644 index cba3444..0000000 --- a/src/GravLib/drivebase/drivetrain.rs +++ /dev/null @@ -1,20 +0,0 @@ -use crate::GravLib::actuator::MotorGroup; - -/// The raw motor‐&‐geometry bundle. -pub struct Drivetrain { - pub left_motors: MotorGroup, - pub right_motors: MotorGroup, - pub track_width: f64, - pub wheel_diameter: f64, -} - -impl Drivetrain { - pub fn new( - left: MotorGroup, - right: MotorGroup, - track_width: f64, - wheel_diameter: f64, - ) -> Self { - Self { left_motors: left, right_motors: right, track_width, wheel_diameter } - } -} diff --git a/src/GravLib/drivebase/mod.rs b/src/GravLib/drivebase/mod.rs deleted file mode 100644 index c4ce290..0000000 --- a/src/GravLib/drivebase/mod.rs +++ /dev/null @@ -1,6 +0,0 @@ -pub use chassis::Chassis; -pub use drivetrain::Drivetrain; - -mod chassis; -mod drivetrain; -pub mod driver; diff --git a/src/GravLib/misc.rs b/src/GravLib/misc.rs new file mode 100644 index 0000000..f6dcaf2 --- /dev/null +++ b/src/GravLib/misc.rs @@ -0,0 +1,63 @@ +use vexide::devices::math::Point2; +use vexide::prelude::*; +use vexide::devices::{display::*}; + +pub fn gravlib_logo(display: &mut Display) { + // common colours & x‑position + let yellow = Rgb::new(255, 255, 0); + let black = Rgb::new(0, 0, 0); + let x: i16 = 10; + + // 1) Top banner, art rows, and bottom banner + let mut y: i16 = 15; + let banner_rows: &[(&str, FontSize)] = &[ + ("################################################", FontSize::SMALL), + ("", FontSize::EXTRA_SMALL), + (" $$$$$$\\ $$\\ $$\\ $$\\ ", FontSize::EXTRA_SMALL), + ("$$ __$$\\ $$ | \\__|$$ | ", FontSize::EXTRA_SMALL), + ("$$ / \\__| $$$$$$\\ $$$$$$\\ $$\\ $$\\ $$ | $$\\ $$$$$$$\\ ", FontSize::EXTRA_SMALL), + ("$$ |$$$$\\ $$ __$$\\ \\____$$\\\\$$\\ $$ |$$ | $$ |$$ __$$\\ ", FontSize::EXTRA_SMALL), + ("$$ |\\_$$ |$$ | \\__|$$$$$$$ |\\$$\\$$ / $$ | $$ |$$ | $$ |", FontSize::EXTRA_SMALL), + ("\\$$$$$$ |$$ | \\$$$$$$$ | \\$ / $$$$$$$$\\ $$ |$$$$$$$ |", FontSize::EXTRA_SMALL), + (" \\______/ \\__| \\_______| \\_/ \\________|\\__|\\_______/ ", FontSize::EXTRA_SMALL), + ("", FontSize::EXTRA_SMALL), + ("################################################", FontSize::SMALL), + ]; + + for &(line, size) in banner_rows { + let font = Font::new(size, FontFamily::Monospace); + display.draw_text( + &Text::new(line, font, Point2::::from([x, y])), + yellow, + Some(black), + ); + y += match size { + FontSize::SMALL => 15, + FontSize::EXTRA_SMALL => 10, + _ => 12, + }; + } + + // 2) Description block + let desc_base_y = y + 15; + let desc_spacing = 15; + let description = [ + "", + " GravLib v0.1 (Pre-release Version)", + " By Alex Cai (LycoKodo) & GravLib Contributors", + " \"All Hail Nijika Ijichi\"", + ]; + + for (i, &line) in description.iter().enumerate() { + let yy = desc_base_y + (i as i16 * desc_spacing); + display.draw_text( + &Text::new( + line, + Font::new(FontSize::SMALL, FontFamily::Monospace), + Point2::::from([x, yy]), + ), + yellow, + Some(black), + ); + } +} diff --git a/src/GravLib/mod.rs b/src/GravLib/mod.rs index fa0cf02..e7da4f6 100644 --- a/src/GravLib/mod.rs +++ b/src/GravLib/mod.rs @@ -1,7 +1,10 @@ pub mod actuator; pub mod pid; -pub mod drivebase; +pub mod subsystems; +pub mod odom; +pub mod misc; +pub mod motions; pub use pid::PID; pub use pid::Gains; \ No newline at end of file diff --git a/src/GravLib/motions/mod.rs b/src/GravLib/motions/mod.rs new file mode 100644 index 0000000..f1c6507 --- /dev/null +++ b/src/GravLib/motions/mod.rs @@ -0,0 +1 @@ +mod motion_cancel_helper; \ No newline at end of file diff --git a/src/GravLib/motions/motion_cancel_helper.rs b/src/GravLib/motions/motion_cancel_helper.rs new file mode 100644 index 0000000..03b1011 --- /dev/null +++ b/src/GravLib/motions/motion_cancel_helper.rs @@ -0,0 +1,57 @@ +use vexide::time::{sleep_until, Instant}; +use vexide::competition; +use core::time::Duration; +use vexide::task; + +pub struct MotionCancelHelper { + m_firstIteration: bool, + m_prevTime: Instant, + m_originalCompStatus: competition::CompetitionMode, + m_period: Duration +} + +/** + * const int64_t now = int64_t(pros::millis()); + if (now - int64_t(m_prevTime) > int64_t(processedTimeout)) m_prevTime = now - processedTimeout; + // only delay if this is not the first iteration + if (!m_firstIteration) pros::Task::delay_until(&m_prevTime, processedTimeout); + else m_firstIteration = false; + */ + +impl MotionCancelHelper { + pub fn new(period: Duration) -> Self { + let status = competition::status().mode(); + MotionCancelHelper { + m_firstIteration: true, + m_prevTime: Instant::now() - period, + m_originalCompStatus: status, + m_period: period + } + } + + pub fn wait(&mut self) -> bool { + let mut m_prevPrevTime: Instant = self.m_prevTime; + let processedTimeout: Duration = self.m_period; + + let status = competition::status().mode(); + + let now = Instant::now(); + if now - self.m_prevTime > processedTimeout { + self.m_prevTime = now - processedTimeout; + } + + // delay if not first iteration + if !self.m_firstIteration { + sleep_until(self.m_prevTime + processedTimeout); + } else { + self.m_firstIteration = false; + } + + // different competition status as started, exit motion + if status != self.m_originalCompStatus { + false // end motion + } + + + } +} \ No newline at end of file diff --git a/src/GravLib/motions/turn_to.rs b/src/GravLib/motions/turn_to.rs new file mode 100644 index 0000000..6113d70 --- /dev/null +++ b/src/GravLib/motions/turn_to.rs @@ -0,0 +1,28 @@ +use crate::Gravlib::PID; + +enum locked_side{ + LEFT, + RIGHT, +} + +enum angular_direction{ + CLOCKWISE, + COUNTERCLOCKWISE, +} + +struct turn_to_params{ + config: Option, + direction: Option, + maxSpeed: Option, // TODO - Default = 1 + minSpeedL: Option, // TODO - Default = 0 + slew: Option, // TODO - Default = 0 + earlyExitRange: Option, // TODO - 0 degrees +} + +struct turn_to_settings{ + pid: PID, +} + +fn turnTo(target: f64, timeout: f64, params: turn_to_params, turn_to_settings: turn_to_settings) { + +} \ No newline at end of file diff --git a/src/GravLib/odom/localisation.rs b/src/GravLib/odom/localisation.rs new file mode 100644 index 0000000..c1618ff --- /dev/null +++ b/src/GravLib/odom/localisation.rs @@ -0,0 +1,274 @@ +use core::time::Duration; + +use vexide::devices::adi::digital; +use vexide::{ + devices::smart::InertialSensor, +}; + +use alloc::{sync::Arc, vec::Vec}; +use vexide::io::println; +use spin::Mutex; +use alloc::vec; + + +use uom::{si::f64::Angle, ConstZero}; + +use libm; + +use vexide::devices::math::Point2; +use vexide::prelude::*; +use vexide::devices::display::{self, *}; + +use crate::GravLib::odom::sensors::{TrackingWheel, Sensors}; + +use alloc::format; + +pub struct Pose { + x: f64, + y: f64, + theta: f64, // in degrees +} + +impl Pose { + pub fn new() -> Self { + Self { + x: 0.0, + y: 0.0, + theta: 0.0, + } + } + + pub fn get_position(&self) -> (f64, f64, f64) { + (self.x, self.y, self.theta) + } +} + +pub struct Localisation { + pub sensors: Arc>, + pub m_pose: Arc>, + prev_vertical_total: Vec, + prev_horizontal_total: Vec, +} + +fn calculate_wheel_heading(wheels: &Vec>>) -> f64 { + // if not enough wheels + if wheels.len() < 2 { + return 0.0; // or some default value + } + + // get data + let distance1 = wheels[0].lock().get_distance_travelled(); + let distance2 = wheels[1].lock().get_distance_travelled(); + + let offset1 = wheels[0].lock().get_offset(); + let offset2 = wheels[1].lock().get_offset(); + + // TODO - Add error logic for: + // [] - equal offsets + // [] - error wheel readings + + // calculate heading + ((distance1 - distance2) / (offset1 - offset2)) + 90.0 +} + +//TODO - Add thing to consider faulty sensors +fn find_lateral_delta(sensors: Vec>>) -> f64 { + let mut data_vec = Vec::new(); + for i in 0..sensors.len() { + let sensor = sensors[i].lock(); + let data = sensor.get_distance_travelled(); + // TODO - Handle fault sensor. + data_vec.push(data); + } + + return if !data_vec.is_empty() { + data_vec.iter().sum::() / data_vec.len() as f64 + } else { + 0.0 // Return 0.0 if no data was found + }; +} + +/// Compute the robot’s local (Δx, Δy) given wheel deltas & offsets and a rotation Δθ. +fn compute_local_position( + delta_theta: f64, + vertical_delta: f64, + horizontal_delta: f64, + vertical_offset: f64, + horizontal_offset: f64, +) -> (f64, f64) { + let lateral_deltas = (vertical_delta, horizontal_delta); + let lateral_offsets = (vertical_offset, horizontal_offset); + + if delta_theta == 0.0 { + // straight‐line case + lateral_deltas + } else { + // chord‐length formula on each axis + let factor = 2.0 * libm::sin(delta_theta * 0.5); + let inv_theta = 1.0 / delta_theta; + let radius = ( + lateral_deltas.0 * inv_theta + lateral_offsets.0, + lateral_deltas.1 * inv_theta + lateral_offsets.1, + ); + (factor * radius.0, factor * radius.1) + } +} + +/// Reject outliers from the deltas and return the average of the remaining values. +fn reject_outliers_and_average(deltas: &Vec, threshold: f64) -> f64 { + if deltas.is_empty() { + return 0.0; + } + + let mean = deltas.iter().copied().sum::() / deltas.len() as f64; + let variance = deltas.iter().map(|d| (d - mean).powi(2)).sum::() / deltas.len() as f64; + let std_dev = variance.sqrt(); + + let filtered: Vec = deltas + .iter() + .copied() + .filter(|d| (d - mean).abs() <= threshold * std_dev) + .collect(); + + if filtered.is_empty() { + 0.0 // fallback if all are outliers + } else { + filtered.iter().sum::() / filtered.len() as f64 + } +} + + +impl Localisation { + pub fn new(sensors: Arc>) -> Self { + // Pre‑allocate space to store the last total for each wheel + let num_v = sensors.lock().vertical_wheels.len(); + let num_h = sensors.lock().horizontal_wheels.len(); + Self { + sensors, + m_pose: Arc::new(Mutex::new(Pose::new())), + prev_vertical_total: vec![0.0; num_v], + prev_horizontal_total: vec![0.0; num_h], + } + } + + pub async fn calibrate(&mut self, calibrate_imu: bool) { + if calibrate_imu { + println!("Calibrating IMU..."); + self.sensors.lock().imu.lock().calibrate().await; + println!("IMU calibration complete."); + } else { + println!("Skipping IMU calibration. (User Specified)"); + } + + self.sensors.lock().horizontal_wheels.iter().for_each(|w| { + w.lock().reset(); + }); + + self.sensors.lock().vertical_wheels.iter().for_each(|w| { + w.lock().reset(); + }); + println!("All sensors reset."); + + } + + pub fn update(&mut self, display: &mut Display) { + loop { + // 1. Read *deltas* from each wheel + let mut vertical_deltas = Vec::new(); + let mut horizontal_deltas = Vec::new(); + + { + let s = self.sensors.lock(); + + // --- Vertical wheels --- + for (i, w) in s.vertical_wheels.iter().enumerate() { + let total = w.lock().get_distance_travelled(); + let delta = total - self.prev_vertical_total[i]; + self.prev_vertical_total[i] = total; + vertical_deltas.push(delta); + } + + // --- Horizontal wheels --- + for (i, w) in s.horizontal_wheels.iter().enumerate() { + let total = w.lock().get_distance_travelled(); + let delta = total - self.prev_horizontal_total[i]; + self.prev_horizontal_total[i] = total; + horizontal_deltas.push(delta); + } + } + + // Reject outliers and average the rest + let vertical_delta = reject_outliers_and_average(&vertical_deltas, 1.5); + let horizontal_delta = reject_outliers_and_average(&horizontal_deltas, 1.5); + + + // 2. Correctly pick up theta and *assign* it + let mut theta = 0.0; + if let Ok(imu_heading) = self.sensors.lock().imu.lock().heading() { + theta = imu_heading; + } else { + println!("IMU WENT WRONG!!"); + } + + // 3. Convert heading difference into *radians* for the chord formula + let old_theta = self.m_pose.lock().theta; + let delta_theta_deg = theta - old_theta; + let delta_theta_rad = delta_theta_deg.to_radians(); + + // 4. Compute local Δx/Δy using the chord formula + let vertical_offset = self.sensors.lock().vertical_wheels[0].lock().get_offset(); + let horizontal_offset = self.sensors.lock().horizontal_wheels[0].lock().get_offset(); + + println!("Delta θ: {:}°", delta_theta_rad); + let (delta_x, delta_y) = if delta_theta_rad.abs() == 0.0 { + // straight line + // println!("STRAIGHT"); + (horizontal_delta, vertical_delta) // Swapped: X gets horizontal, Y gets vertical + } else { + let factor = 2.0 * libm::sin(delta_theta_rad * 0.5); + let inv_theta = 1.0 / delta_theta_rad; + let r_x = horizontal_delta * inv_theta + horizontal_offset; // X uses horizontal wheel + let r_y = vertical_delta * inv_theta + vertical_offset; // Y uses vertical wheel + (factor * r_x, factor * r_y) + }; + + // 5. Rotate into the global frame + let mid_heading = (old_theta + delta_theta_deg * 0.5).to_radians(); + let cos_h = libm::cos(mid_heading); + let sin_h = libm::sin(mid_heading); + let global_dx = delta_x * cos_h + delta_y * sin_h; + let global_dy = delta_x * sin_h + delta_y * cos_h; + + // 6. Update your pose + { + let mut pose = self.m_pose.lock(); + pose.x += global_dx; + pose.y += global_dy; + pose.theta = theta; + // println!( + // "Pose → x: {:+.4}, y: {:+.4}, θ: {:.2}°", + // pose.x, pose.y, pose.theta + // ); + } + + let pose = self.m_pose.lock(); + let (x, y, theta) = pose.get_position(); + drop(pose); + + display.set_render_mode(RenderMode::DoubleBuffered); + display.erase(Rgb::new(0, 0, 0)); + display.draw_text( + &Text::new( + &format!("X: {:+.4}\nY: {:+.4}\nθ: {:.2}°", x, y, theta), + Font::new(FontSize::MEDIUM, FontFamily::Monospace), + Point2::::from([10, 10]), + ), + Rgb::new(255, 255, 255), + Some(Rgb::new(0, 0, 0)), + ); + + // 7. Sleep until next tick + vexide::time::sleep(Duration::from_millis(10)); + } + } +} diff --git a/src/GravLib/odom/mod.rs b/src/GravLib/odom/mod.rs new file mode 100644 index 0000000..ba36ff5 --- /dev/null +++ b/src/GravLib/odom/mod.rs @@ -0,0 +1,2 @@ +pub mod sensors; +pub mod localisation; \ No newline at end of file diff --git a/src/GravLib/odom/sensors.rs b/src/GravLib/odom/sensors.rs new file mode 100644 index 0000000..0e1720d --- /dev/null +++ b/src/GravLib/odom/sensors.rs @@ -0,0 +1,51 @@ +use vexide::{ + devices::smart::RotationSensor, + devices::smart::InertialSensor, +}; + +use core::f64::consts::PI; +use alloc::{sync::Arc, vec::Vec}; +use spin::Mutex; + +pub struct Sensors { + pub horizontal_wheels: Vec>>, + pub vertical_wheels: Vec>>, + pub imu: Arc>, +} + +pub struct TrackingWheel { + rotation: RotationSensor, + diameter: f64, + offset: f64, + ratio: f64, +} + +impl TrackingWheel { + pub fn new(rotation: RotationSensor, diameter: f64, offset: f64, ratio: f64) -> Self { + Self { + rotation, + diameter, + offset, + ratio: if ratio == 0.0 { 1.0 } else { ratio }, + } + } + + /// Returns the distance travelled in the same units as `diameter`. + pub fn get_distance_travelled(&self) -> f64 { + // Assume get_angle() returns the angle in degrees. + let angle_degrees = self.rotation.position().unwrap().as_degrees(); + + angle_degrees * self.diameter * PI / 360.0 + } + + /// Returns the offset of the tracking wheel. + pub fn get_offset(&self) -> f64 { + self.offset + } + + /// Resets the sensor and tracking wheel. + /// We assume that rotation.reset() resets the internal counter and returns an i64 status code. + pub fn reset(&mut self) { + self.rotation.reset_position(); + } +} \ No newline at end of file diff --git a/src/GravLib/subsystems/drivetrain.rs b/src/GravLib/subsystems/drivetrain.rs new file mode 100644 index 0000000..bd4f953 --- /dev/null +++ b/src/GravLib/subsystems/drivetrain.rs @@ -0,0 +1,25 @@ +use alloc::{sync::Arc, vec::Vec}; +use core::{f64::consts::TAU, ops::Add, time::Duration}; + +use vexide::{ + core::{sync::Mutex, time::Instant}, +}; + +use crate::{ + actuator::MotorGroup, +}; + +#[allow(dead_code)] +pub struct DriveTrain { + left: Arc>, + right: Arc>, + _odom_task: Task<()>, +} + +impl DriveTrain { + pub async fn new( + left_motors: Arc>>, + right_motors: Arc>>, + imu: InertialSensor + ) +} \ No newline at end of file diff --git a/src/GravLib/subsystems/mod.rs b/src/GravLib/subsystems/mod.rs new file mode 100644 index 0000000..e69de29 diff --git a/src/main.rs b/src/main.rs index 45950cc..9b7d5b7 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,22 +4,94 @@ extern crate alloc; mod GravLib; use core::time::Duration; -use spin::Mutex; -use alloc::vec::Vec; -use alloc::boxed::Box; -use vexide::{devices::adi::motor, prelude::*}; +use alloc::{sync::Arc, vec::Vec}; +use alloc::vec; +use spin::Mutex; -use crate::GravLib::actuator::MotorGroup; -use crate::GravLib::drivebase::Chassis; +use vexide::devices::math::Point2; +use vexide::prelude::*; +use vexide::devices::{display::*}; -use crate::GravLib::PID; +use crate::GravLib::misc::gravlib_logo; +use crate::GravLib::odom::{ + sensors::{TrackingWheel, Sensors}, + localisation::Localisation +}; struct Robot { - chassis: Chassis, controller: Controller, + display: Arc>, + localisation: Arc>, +} + +impl Robot { + pub fn new(peripherals: Peripherals) -> Self { + // Sensor configuration placeholders + let vertical_wheel = Arc::new(Mutex::new(TrackingWheel::new( + RotationSensor::new(peripherals.port_10, Direction::Forward), // PLACEHOLDER: Configure vertical tracking wheel port + 2.75, // PLACEHOLDER: Set wheel diameter in inches + 0.0, // PLACEHOLDER: Set vertical offset from center in inches + 1.0, // PLACEHOLDER: Set gear ratio + ))); + + let horizontal_wheel = Arc::new(Mutex::new(TrackingWheel::new( + RotationSensor::new(peripherals.port_9, Direction::Forward), // PLACEHOLDER: Configure horizontal tracking wheel port + 2.75, // PLACEHOLDER: Set wheel diameter in inches + -4.5, // PLACEHOLDER: Set horizontal offset from center in inches + 1.0, // PLACEHOLDER: Set gear ratio + ))); + + let sensors = Arc::new(Mutex::new(Sensors { + horizontal_wheels: vec![horizontal_wheel], + vertical_wheels: vec![vertical_wheel], + imu: Arc::new(Mutex::new(InertialSensor::new(peripherals.port_7))), // PLACEHOLDER: Configure IMU port + })); + + let localisation = Arc::new(Mutex::new(Localisation::new(sensors))); + let display = Arc::new(Mutex::new(peripherals.display)); + + Self { + controller: peripherals.primary_controller, + display, + localisation, + } + } + + #[allow(dead_code)] + pub async fn initialise(&mut self) { + // 1. Calibrate IMU + println!("Robot calibration Started."); + + // 2. Draw the gravlib logo on the display + { + let mut d = self.display.lock(); + gravlib_logo(&mut *d); + } + self.localisation.lock().calibrate(true).await; + + println!("Robot calibration complete."); + + + // 3. Spawn a background task for continual localisation updates & telemetry + let loc = Arc::clone(&self.localisation); + let disp = Arc::clone(&self.display); + + vexide::task::spawn(async move { + loop { + { + let mut local = loc.lock(); + let mut d = disp.lock(); + local.update(&mut *d); + } + // sleep between updates + let _ = vexide::time::sleep(Duration::from_millis(10)).await; + } + }).detach(); + } } + impl Compete for Robot { async fn autonomous(&mut self) { // your autonomous code here @@ -28,34 +100,27 @@ impl Compete for Robot { async fn driver(&mut self) { // driver-control loop: reads sticks & sets voltages + // loop { + // // Get current robot position from localization + // let pose = self.localisation.lock(); + // let (x, y, theta) = pose.lock().get_position(); + + // println!("Robot Position - X: {:.4}, Y: {:.4}, Theta: {:.4}°", x, y, theta); + + // //delay 10ms + // vexide::time::sleep(Duration::from_millis(10)).await; + // } loop { - self.chassis.tank_drive(&self.controller); - // yield to the runtime so the controller-update task can run - sleep(Controller::UPDATE_INTERVAL).await; + vexide::time::sleep(Duration::from_millis(10)).await; } } } #[vexide::main] async fn main(peripherals: Peripherals) { - // Build two groups of three motors each - let left = MotorGroup::new(Vec::from([ - Motor::new(peripherals.port_1, Gearset::Green, Direction::Reverse), - Motor::new(peripherals.port_2, Gearset::Green, Direction::Reverse), - Motor::new(peripherals.port_3, Gearset::Green, Direction::Reverse), - ])); - - let right = MotorGroup::new(Vec::from([ - Motor::new(peripherals.port_4, Gearset::Green, Direction::Forward), - Motor::new(peripherals.port_5, Gearset::Green, Direction::Forward), - Motor::new(peripherals.port_6, Gearset::Green, Direction::Forward), - ])); - - // Wrap them in your chassis - let chassis = Chassis::new(left, right, 320.0, 100.0); - let controller = peripherals.primary_controller; - - let mut robot = Robot { chassis, controller }; - // This hands off control to vexide’s scheduler (autonomous → driver) + let mut robot = Robot::new(peripherals); + robot.initialise().await; + + // This hands off control to vexide's scheduler (autonomous → driver) robot.compete().await; }