60 lines
2.2 KiB
Rust
60 lines
2.2 KiB
Rust
use rnavp::motor::{self, Direction};
|
|
use rnavp::pid;
|
|
use uom::si::f32::AngularVelocity;
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
/// # Motor Controller
|
|
/// This is a motor controller that takes in a generic type T that has implemented the needed traits.
|
|
/// Most specifically Driver and Sensor.
|
|
///
|
|
/// ## Usage
|
|
///
|
|
/// Creating this allows for the controller a motor via a simple PID f32 signed Speed command, rather than u16 speed commands
|
|
/// Notice that this also TAKES the motor from your manual controller, you loose access to the manual hand control you had before.
|
|
pub struct Controller<T>
|
|
where
|
|
T: motor::Driver + motor::Sensor,
|
|
{
|
|
motor: T,
|
|
max_speed: AngularVelocity,
|
|
pid: pid::PID,
|
|
current_direction: Direction,
|
|
}
|
|
|
|
///Controller Impl for the adding the PID logic for handling the motor compiston
|
|
impl<T: motor::Driver + motor::Sensor> Controller<T> {
|
|
///Uses the given set_speed and then will handle the rest of the control logic to accurately* hit the requested speed
|
|
pub async fn control(&mut self, set_speed: AngularVelocity) {
|
|
let max_speed_rads = self.max_speed.value;
|
|
let set_speed_rads = set_speed.value.clamp(-max_speed_rads, max_speed_rads);
|
|
|
|
self.pid.set_point(set_speed_rads).await;
|
|
let pid_output = self.pid.pid_step(self.motor.get_speed().await.value).await;
|
|
|
|
self.current_direction.dir_from_f32(pid_output);
|
|
|
|
let motor_command = (pid_output * 65535.0) as u16;
|
|
|
|
self.motor
|
|
.set_speed_and_direction(motor_command, self.current_direction)
|
|
.await;
|
|
}
|
|
|
|
///Retrieve the speed from the motor inside the controller.
|
|
/// This allows you to get a speed value from behind the move.
|
|
pub async fn retrieve(&self) -> AngularVelocity {
|
|
self.motor.get_speed().await
|
|
}
|
|
|
|
///Creates a new Controller with a motor (of type T), a max speed and a pid::Config
|
|
/// This then allows for you to use the controller with the provided types
|
|
pub fn new(motor: T, max_speed: AngularVelocity, config: pid::Config) -> Self {
|
|
Controller {
|
|
motor: motor,
|
|
max_speed: max_speed,
|
|
pid: pid::PID::new(config),
|
|
current_direction: Direction::CCW,
|
|
}
|
|
}
|
|
}
|