500 lines
14 KiB
Lua
500 lines
14 KiB
Lua
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<T: MotorGroup, C: Config = DefaultConfig> {
|
|
pub left_motor: T,
|
|
pub right_motor: T,
|
|
pub config: C,
|
|
}
|
|
impl<T: MotorGroup, C: Config> DriveTrain for Drive<T, C> {
|
|
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_while<F:FnMut()->bool>(
|
|
&mut self,
|
|
sensor:&Mutex<InertialSensor>,
|
|
position:&Mutex<Point>,
|
|
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<T:Motor>{
|
|
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<f64>,
|
|
max: Option<f64>,
|
|
}
|
|
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<f64>) -> 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<f64>) -> 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<f64> 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<f64> 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<const N: usize>{
|
|
// 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<const N: usize> AutoTune<N> {
|
|
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<PidWeights> {
|
|
// 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",
|
|
}
|
|
}
|
|
|
|
}
|