return { { "letieu/hacker.nvim", opts = { content = [[//! # Functionality for making drivetrains and includes some basic ones. pub mod motors; // pub mod no_block; use core::{ f64::consts::{TAU, PI}, ops::{ RangeInclusive, Add, AddAssign, Sub, SubAssign, Mul, Div, }, time::Duration, }; use libm::{ sqrt, pow, }; use alloc::{ vec::Vec, vec, }; use crate::{prelude::*, println_blue}; use motors::MotorGroup; pub struct Drive { pub left_motor: T, pub right_motor: T, pub config: C, } impl DriveTrain for Drive { type Config = C; type MotorError = T::MotorError; fn move_i8(&mut self, l: i8, r: i8) -> Result<(), T::MotorError> { self.left_motor.move_i8(l)?; self.right_motor.move_i8(r) } fn move_voltage(&mut self, l: i32, r: i32) -> Result<(), T::MotorError> { self.left_motor.move_voltage(l)?; self.right_motor.move_voltage(r) } fn config(&self) -> &Self::Config { &self.config } } /// Represents the drivetrain of the robot pub trait DriveTrain { type Config: Config; type MotorError; ///moves the sides of the drivetrain the specified voltadge fn move_voltage(&mut self, left: i32, right: i32) -> Result<(), Self::MotorError>; ///moves the sides of the drivetrain the specified number fn move_i8(&mut self, left: i8, right: i8) -> Result<(), Self::MotorError>; fn config(&self) -> &Self::Config; fn drive(&mut self, y: i8, x: i8) -> Result<(), Self::MotorError> { let (left, right) = self.config().drive_math(y, x); self.move_i8(left, right)?; Ok(()) } /// one frame of a pid loop, moves twards specified point fn go_to( &mut self, heading: Angle, init: Point, to: Point, pids: &mut PidPair, ) -> Result<(), Self::MotorError> { let (frwdpidin, turnpidin) = Self::Config::go_to_pid_math(heading, init, to); pids.0.add(frwdpidin); pids.1.add(turnpidin); let frwd = pids.0.get() as i32; let turn = pids.1.get() as i32; self.move_voltage(frwd - turn, frwd + turn)?; Ok(()) } /// blocks, drives the drivetrain forward to a point until the condition becomes false fn go_to_whilebool>( &mut self, sensor:&Mutex, position:&Mutex, to:Point, pids:&mut PidPair, ctx:Context, mut cond:F, ) -> Result<(), Self::MotorError> { let mut l = Loop::new(Duration::from_millis(10)); while cond() { self.go_to( Angle::from_imu_read(sensor.lock().get_heading().warn_default()), position.lock().clone(), to.clone(), pids )?; select! { () = l.select() => continue, () = ctx.done() => break, } } self.move_voltage(0, 0); Ok(()) } /// one frame of a pid loop, turns twards the specified point fn turn_to( &mut self, heading: Angle, init: Point, to: Point, pid: &mut Pid, reverse:bool ) -> Result<(), Self::MotorError> { let delta_heading = match reverse { false => init.angle_between(&to) - heading, true => init.angle_between(&to) + Angle::from_rad(PI) - heading, }; pid.add(delta_heading.as_rad_neg()); let mvolt = pid.get() as i32; self.move_voltage(-mvolt, mvolt) } /// moves the sides of the drivetrain the specified voltadge, then waits for time fn go_for(&mut self, left: i32, right: i32, time: Duration) -> Result<(), Self::MotorError> { self.move_voltage(left, right)?; Task::delay(time); Ok(()) } } /// Represendts two pids controlling the forward (l, _), and turn (_, r) components of a drivetrain pub type PidPair = (Pid, Pid); /* /// Four wheeled holonomic drivetrain struct. pub struct Holo{ rightfronf:T, leftfront:T, rightback:T, leftback:T, } /// WIP pub trait Holonomic { fn forward_math(&self, x:i8, y:i8, imu:Angle); fn turn_math(&mut self, turn:i8); } */ /// Anything with the ability to be configured as a driver profile. pub trait Config { ///the math for how the drivetrain moves given stick input fn drive_math(&self, y: i8, x: i8) -> (i8, i8); /// the math for the values that the pids should be passed in when going to /// point (frwd, turn). fn go_to_pid_math(heading: Angle, init: Point, to: Point) -> (f64, f64) { let change = to - init; let relitive = change.rotate(-heading); (relitive.x(), relitive.y()) } } pub struct DefaultConfig; impl Config for DefaultConfig { fn drive_math(&self, y: i8, x: i8) -> (i8, i8) { //we want to truncate here #[allow(clippy::cast_possible_truncation)] let left = ((y as i16) + (x as i16)).clamp(-127, 127) as i8; #[allow(clippy::cast_possible_truncation)] let right = ((y as i16) - (x as i16)).clamp(-127, 127) as i8; (left, right) } } fn signum(n:f64)->f64{ if n<0.0{ -1.0 } else{ 1.0 } } ///Contains fuctionality for smooth acceleration to a destination /// /// # examples /// /// ``` /// #![no_std] /// #![no_main] /// /// struct MyRobot { /// left_motor: Motor, /// right_motor: Motor, /// pid: Pid, /// } /// impl MyRobot { /// ///moves the robot forward the specified number of ticks /// fn move_dist(&mut self, distance: f64) -> Result<(), MotorError> { /// self.left_motor.tare_position().unwrap(); /// self.right_motor.tare_position().unwrap(); /// self.pid.add(distance); /// loop { /// self.left_motor /// .move_voltadge(self.pid.get_weight()) /// .unwrap(); /// self.right_motor /// .move_voltadge(self.pid.get_weight()) /// .unwrap(); /// let err = dist - self.left_motor.get_position().unwrap(); /// self.pid.add(err); /// if err == 0.0 { /// break; /// } /// } /// } /// } /// ``` #[derive(Debug, Clone)] pub struct Pid { kp: f64, ki: f64, kd: f64, p: f64, i: f64, d: f64, range: RangeInclusive, max: Option, } impl Pid { /// constructs new pid with the given weights, and the allowed range of the /// internal values pub const fn new(weights:PidWeights, range: RangeInclusive) -> Self { Self { kp: weights.kp, ki: weights.ki, kd: weights.kd, p: 0.0, i: 0.0, d: 0.0, range, max: None, } } pub fn add(&mut self, new: f64) { self.d = self.p - new; self.i += new; self.p = new; self.p = self.p.clamp(*self.range.start(), *self.range.end()); self.i = self.i.clamp(*self.range.start(), *self.range.end()); self.d = self.d.clamp(*self.range.start(), *self.range.end()); } pub fn get(&self) -> f64 { let max = self.max.unwrap_or(f64::INFINITY); ((self.p * self.kp) + (self.i * self.ki) + (self.d * self.kd)).clamp(-max, max) } pub const fn set_max(mut self, max: Option) -> Self { self.max = max; self } pub fn reset(&mut self) { self.p = 0.0; self.i = 0.0; self.d = 0.0; } pub fn weights(&self) -> PidWeights { PidWeights { kp: self.kp, ki: self.ki, kd: self.kd, } } pub fn set_weights(&mut self, weights:PidWeights) { self.kp = weights.kp; self.ki = weights.ki; self.kd = weights.kd; } } #[derive(Debug, Clone, PartialEq)] pub struct PidWeights { pub kp: f64, pub ki: f64, pub kd: f64, } impl PidWeights { fn positive(&self) ->Self{ Self{ kp: self.kp.max(0.0), ki: self.ki.max(0.0), kd: self.kd.max(0.0), } } fn max(&self, rhs: &Self) ->Self{ Self{ kp: self.kp.max(-rhs.kp), ki: self.ki.max(-rhs.ki), kd: self.kd.max(-rhs.kd), } } fn max_magnitude(&self, mag: f64) -> Self { let self_mag = self.magnitude(); if self_mag > mag { &(self*mag)/self_mag } else { self.clone() } } fn magnitude(&self)->f64{ sqrt(pow(self.kp, 2.0) + pow(self.ki, 2.0) + pow(self.kd, 2.0)) } /// # NOTE: self is three independent vectors, not one fn gradient_acent(self, responce: (f64, f64, f64)) -> PidWeights { PidWeights{ kp: responce.0/self.kp, ki: responce.1/self.ki, kd: responce.2/self.kd } } } impl Add for PidWeights { type Output = Self; fn add(self, rhs: Self) -> Self::Output { Self { kp: self.kp + rhs.kp, ki: self.ki + rhs.ki, kd: self.kd + rhs.kd } } } impl Sub for PidWeights { type Output = Self; fn sub(self, rhs: Self) -> Self::Output { Self { kp: self.kp - rhs.kp, ki: self.ki - rhs.ki, kd: self.kd - rhs.kd } } } impl Add for &PidWeights { type Output = PidWeights; fn add(self, rhs: Self) -> Self::Output { PidWeights { kp: self.kp + rhs.kp, ki: self.ki + rhs.ki, kd: self.kd + rhs.kd } } } impl Sub for &PidWeights { type Output = PidWeights; fn sub(self, rhs: Self) -> Self::Output { PidWeights { kp: self.kp - rhs.kp, ki: self.ki - rhs.ki, kd: self.kd - rhs.kd } } } impl Mul for &PidWeights { type Output = PidWeights; fn mul(self, rhs: f64) -> Self::Output { PidWeights { kp: self.kp * rhs, ki: self.ki * rhs, kd: self.kd * rhs } } } impl Div for &PidWeights { type Output = PidWeights; fn div(self, rhs: f64) -> Self::Output { PidWeights { kp: self.kp / rhs, ki: self.ki / rhs, kd: self.kd / rhs } } } impl AddAssign for PidWeights { fn add_assign(&mut self, rhs: Self) { *self = &*self + &rhs } } impl SubAssign for PidWeights { fn sub_assign(&mut self, rhs: Self) { *self = &*self - &rhs } } #[derive(Default, Clone, Debug)] enum Weight { All, Kp, Ki, #[default] Kd, } impl Weight { fn next(&self) -> Self{ match self { Self::All => Self::Kp, Self::Kp => Self::Ki, Self::Ki => Self::Kd, Self::Kd => Self::All, } } } /// automagicaly tunes pids to maximize v #[derive(Debug, Clone, Default)] pub struct AutoTune{ // keep track of all readings for human review readings:Vec<(PidWeights, f64)>, current:Vec<(PidWeights, f64)>, /// the changes that was determened best (not indivudual values) /// will only be none when algorithm hasnt run last_target: Weight, } impl AutoTune { pub fn new() -> Self{ assert_ne!(N, 0, "N must not be zero"); Self::default() } fn last_all(&self) -> &PidWeights { let adjusted_len = self.readings.len() - self.last_target.clone() as usize - 1; &self.readings[adjusted_len].0 } fn last_change(&self) -> Option { // cycles 4 times in order [ kp, ki, kd, all ] let adjusted_len = self.readings.len().checked_sub(self.last_target.clone() as usize)?.checked_sub(1)?; Some( &self.readings.get(adjusted_len)?.0 - &self.readings.get(adjusted_len.checked_sub(4)?)?.0 ) } pub fn tune(&mut self, weight: PidWeights, dist: Distance, time: Duration, alpha: f64, max_magnitude: f64) -> PidWeights{ // GOAL: maximize velocity let vel = dist/time.as_secs_f64(); // in cm/sec if self.current.len() < N { println!("averageing"); self.current.push((weight.clone(), vel)); return weight; } let ave = self.current.iter().fold((PidWeights {kp:0.0, ki:0.0, kd: 0.0}, 0.0), |acc, next| { (&acc.0+&next.0, &acc.1+&next.1) }); self.readings.push((&ave.0/N as f64, ave.1/N as f64)); self.current.clear(); self.last_target = self.last_target.next(); let last_change = self.last_change().unwrap_or_else(||(&weight * alpha).max_magnitude(max_magnitude)); let last_all = self.last_all().clone(); let out = match self.last_target { Weight::All => { PidWeights { kp: last_all.kp + last_change.kp, ..last_all } }, Weight::Kp => { PidWeights { ki: last_all.ki + last_change.ki, ..last_all } }, Weight::Ki => { PidWeights { kd: last_all.kd + last_change.kd, ..last_all } }, Weight::Kd => { let len = self.readings.len(); let last_vel = self.readings[len - 4].1; let delta_reads = (self.readings[len - 3].1-last_vel, self.readings[len - 2].1-last_vel, self.readings[len - 1].1-last_vel); let grad = (&last_change.clone().gradient_acent(delta_reads) * alpha).max_magnitude(max_magnitude); println_blue!("grad={grad:?}\ndelta_reads={delta_reads:?}"); last_all + grad } }; println!("last_change={last_change:?}\nout={out:?}"); out } }]], filetype = "rust", } } }