Changed Motor::Controller from a trait to a Struct
I realized that I was trying to do inheirtance rather than composition which is the rust way. This should be a much bet
This commit is contained in:
+24
-16
@@ -4,30 +4,38 @@ use crate::core::motor::{Direction, Speed};
|
|||||||
use crate::core::logic::pid;
|
use crate::core::logic::pid;
|
||||||
|
|
||||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||||
pub struct Data {
|
/// # Motor Controller
|
||||||
pub max_speed: Speed,
|
/// This is a motor controller that takes in a generic type T that has implemented the needed traits.
|
||||||
pub pid: pid::PID,
|
/// Most specifically Driver and Sensor.
|
||||||
pub current_direction: Direction,
|
///
|
||||||
|
/// ## Usage
|
||||||
|
///
|
||||||
|
/// Creating this allows for the controller a motor via a simple PID f32 signed Speed command, rather than u16 speed commands
|
||||||
|
pub struct Controller<T>
|
||||||
|
where
|
||||||
|
T: motor::Driver + motor::Sensor,
|
||||||
|
{
|
||||||
|
pub motor: T,
|
||||||
|
max_speed: Speed,
|
||||||
|
pid: pid::PID,
|
||||||
|
current_direction: Direction,
|
||||||
}
|
}
|
||||||
|
|
||||||
///Controller trait for motors. Motors with this trait, automatically have controls included
|
///Controller Impl for the adding the PID logic for handling the motor compiston
|
||||||
pub trait Controller: motor::Driver + motor::Sensor {
|
impl<T: motor::Driver + motor::Sensor> Controller<T> {
|
||||||
///Create a Data field using the Data type in your motor struct, and return it here. The rest of the trait is automatic
|
|
||||||
fn data(&mut self) -> Data;
|
|
||||||
|
|
||||||
///Uses the given set_speed and then will handle the rest of the control logic to accurately* hit the requested speed
|
///Uses the given set_speed and then will handle the rest of the control logic to accurately* hit the requested speed
|
||||||
fn control(&mut self, set_speed: Speed) {
|
pub fn control(&mut self, set_speed: Speed) {
|
||||||
let data = &mut self.data();
|
let max_speed_rads = self.max_speed.rads();
|
||||||
let max_speed_rads = data.max_speed.rads();
|
|
||||||
let set_speed_rads = set_speed.rads().clamp(-max_speed_rads, max_speed_rads);
|
let set_speed_rads = set_speed.rads().clamp(-max_speed_rads, max_speed_rads);
|
||||||
|
|
||||||
data.pid.set_point(set_speed_rads);
|
self.pid.set_point(set_speed_rads);
|
||||||
let pid_output = data.pid.pid_step(self.get_speed().rads());
|
let pid_output = self.pid.pid_step(self.motor.get_speed().rads());
|
||||||
|
|
||||||
self.data().current_direction.dir_from_f32(pid_output);
|
self.current_direction.dir_from_f32(pid_output);
|
||||||
|
|
||||||
let motor_command = (pid_output * 65535.0) as u16;
|
let motor_command = (pid_output * 65535.0) as u16;
|
||||||
|
|
||||||
self.set_speed_and_direction(motor_command, data.current_direction);
|
self.motor
|
||||||
|
.set_speed_and_direction(motor_command, self.current_direction);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user