melfely
3df2334b10
changed over all the classes in these files to use async based methods rather than non. This SHOULD make the system more reliable with embassy in the future, and hopefully prevent issues with blocking code and the way embassy works.
130 lines
3.7 KiB
Rust
130 lines
3.7 KiB
Rust
use serde::{Deserialize, Serialize};
|
|
|
|
///The Config for the PID system used by the controller
|
|
#[derive(Serialize, Deserialize, Debug, Default, Clone, Copy, PartialEq)]
|
|
pub struct Config {
|
|
///The Proportional value for the controller
|
|
pub kp: f32,
|
|
///The Integral value for the controller
|
|
pub ki: f32,
|
|
///The Derivative value for the controller
|
|
pub kd: f32,
|
|
///The Feedforward value for the controller
|
|
pub ff: f32,
|
|
///The delta time Value used for the controller
|
|
pub dt: f32,
|
|
///The max accel change from the controller, 0 means no limit
|
|
pub accel: f32,
|
|
}
|
|
|
|
///A PID, the config field is used to handle all the needed configuration.
|
|
/// This PID using pid_step() then attempts to output a value between -1.0 and 1.0 to attempt to make the "current_value" passed in, match the "set_point"
|
|
#[derive(Debug, PartialEq, Clone, Copy)]
|
|
pub struct PID {
|
|
pub config: Config,
|
|
|
|
integral: f32,
|
|
prev_error: f32,
|
|
|
|
set_point: f32,
|
|
|
|
output: f32,
|
|
}
|
|
|
|
impl PID {
|
|
pub fn new(config: Config) -> Self {
|
|
PID {
|
|
config: config,
|
|
integral: 0.0,
|
|
prev_error: 0.0,
|
|
set_point: 0.0,
|
|
output: 0.0,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Default for PID {
|
|
fn default() -> Self {
|
|
PID::new(Config {
|
|
kp: 0.0,
|
|
ki: 0.0,
|
|
kd: 0.0,
|
|
ff: 0.0,
|
|
dt: 0.0,
|
|
accel: 0.0,
|
|
})
|
|
}
|
|
}
|
|
|
|
impl PID {
|
|
///Calculuates a single step of the PID+FF based on the config given to it.
|
|
///Will output a float between -1.0 and 1.0
|
|
///Current value is the current "position" of the system NOT the goal
|
|
/// Use set_point() method to change the "goal position" for the system
|
|
pub async fn pid_step(&mut self, current_value: f32) -> f32 {
|
|
//If the values
|
|
let error = self.set_point - current_value;
|
|
|
|
//If the error is equal to 0.0, just return the output from before. All of this math will just end up not affecting anything
|
|
if error == 0.0 {
|
|
return self.output;
|
|
}
|
|
|
|
//Calculate the integral
|
|
self.integral += error * self.config.dt;
|
|
let i = self.integral * self.config.ki;
|
|
|
|
//Calculate the derivative in a manner that is safe for 0.0 dt
|
|
let d;
|
|
if self.config.dt != 0.0 {
|
|
let derivative = (error - self.prev_error) / self.config.dt;
|
|
d = derivative * self.config.kd;
|
|
} else {
|
|
d = 0.0;
|
|
}
|
|
|
|
//store the original error
|
|
self.prev_error = error;
|
|
|
|
//Calculate the feedforward
|
|
let f = self.set_point * self.config.ff;
|
|
//Calculate the proportional factor
|
|
let p = error * self.config.kp;
|
|
|
|
//Calculate and store the output
|
|
let output = match self.config.accel {
|
|
0.0 => (f + p + i + d) * 0.0001,
|
|
_ => {
|
|
let pre_output = (f + p + i + d) * 0.0001;
|
|
let change = pre_output - self.output;
|
|
let change = change.clamp(-self.config.accel, self.config.accel);
|
|
|
|
let accel_output = self.output + change;
|
|
//Validate that there is a ki value
|
|
if self.config.ki != 0.0 {
|
|
let excess = pre_output - accel_output;
|
|
|
|
self.integral += excess / self.config.ki;
|
|
}
|
|
|
|
accel_output
|
|
}
|
|
};
|
|
|
|
self.output = output.clamp(-1.0, 1.0);
|
|
|
|
//Return the output information
|
|
return self.output;
|
|
}
|
|
|
|
pub async fn set_point(&mut self, set_point: f32) {
|
|
if set_point != self.set_point {
|
|
//Update all internal fields
|
|
self.integral = 0.0;
|
|
self.prev_error = 0.0;
|
|
|
|
self.set_point = set_point;
|
|
}
|
|
}
|
|
}
|