melfely
851ea6c370
There was no way to create a PID, becuase of the private fields I also fixed a divide by 0.0 bug that was 100% going to bite me later if I did not.
129 lines
3.7 KiB
Rust
129 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"
|
|
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 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,
|
|
_ => {
|
|
let pre_output = f + p + i + d;
|
|
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.abs().clamp(-1.0, 1.0);
|
|
|
|
//Return the output information
|
|
return self.output;
|
|
}
|
|
|
|
pub 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;
|
|
}
|
|
}
|
|
}
|