Alex Mikhalev
6 years ago
4 changed files with 400 additions and 29 deletions
@ -0,0 +1,342 @@ |
|||||||
|
use std::sync::atomic::{AtomicBool, Ordering}; |
||||||
|
use std::sync::{Arc, Mutex, MutexGuard}; |
||||||
|
|
||||||
|
pub trait Updatable { |
||||||
|
fn update(&self, dt: f64); |
||||||
|
} |
||||||
|
|
||||||
|
pub type UpdatableRef = Arc<dyn Updatable>; |
||||||
|
|
||||||
|
pub struct NullUpdatable; |
||||||
|
|
||||||
|
impl Updatable for NullUpdatable { |
||||||
|
fn update(&self, _: f64) {} |
||||||
|
} |
||||||
|
|
||||||
|
impl Updatable for Fn(f64) -> () { |
||||||
|
fn update(&self, dt: f64) { |
||||||
|
self(dt) |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl Updatable for [UpdatableRef] { |
||||||
|
fn update(&self, dt: f64) { |
||||||
|
for updatable in self { |
||||||
|
updatable.update(dt); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
use std::time::{Duration, Instant}; |
||||||
|
|
||||||
|
const DEFAULT_PERIOD: Duration = Duration::from_nanos(1e9 as u64 / 60); |
||||||
|
|
||||||
|
pub struct Updater { |
||||||
|
updatable: UpdatableRef, |
||||||
|
period: Duration, |
||||||
|
} |
||||||
|
|
||||||
|
impl Default for Updater { |
||||||
|
fn default() -> Self { |
||||||
|
Self { |
||||||
|
updatable: Arc::new(NullUpdatable), |
||||||
|
period: DEFAULT_PERIOD, |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl Updater { |
||||||
|
pub fn new(updatable: UpdatableRef, period: Duration) -> Self { |
||||||
|
Self { updatable, period } |
||||||
|
} |
||||||
|
|
||||||
|
pub fn run(self) { |
||||||
|
let mut now = Instant::now(); |
||||||
|
let mut last_update = now; |
||||||
|
let zero_duration = Duration::new(0, 0); |
||||||
|
while true { |
||||||
|
let ellapsed = now - last_update; |
||||||
|
let sleep_dur = self.period - ellapsed; |
||||||
|
if sleep_dur < zero_duration { |
||||||
|
warn!( |
||||||
|
"Updater period exceeded ({}ms > {}ms)", |
||||||
|
ellapsed.as_millis(), |
||||||
|
self.period.as_millis() |
||||||
|
); |
||||||
|
} else { |
||||||
|
std::thread::sleep(sleep_dur); |
||||||
|
} |
||||||
|
now = Instant::now(); |
||||||
|
let dt = (now - last_update).as_float_secs(); |
||||||
|
last_update = now; |
||||||
|
self.updatable.update(dt); |
||||||
|
now = Instant::now(); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub trait GenericUpdate { |
||||||
|
type State: Default; |
||||||
|
|
||||||
|
fn enable(&mut self, &mut Self::State); |
||||||
|
fn disable(&mut self, &mut Self::State); |
||||||
|
fn update(&mut self, &mut Self::State, dt: f64); |
||||||
|
} |
||||||
|
|
||||||
|
pub struct GenericUpdatable<T: GenericUpdate> { |
||||||
|
enabled: AtomicBool, |
||||||
|
update_impl: Mutex<T>, |
||||||
|
state: Mutex<T::State>, |
||||||
|
} |
||||||
|
|
||||||
|
impl<T: GenericUpdate + 'static> GenericUpdatable<T> { |
||||||
|
pub fn new(update_impl: T) -> Self { |
||||||
|
Self { |
||||||
|
enabled: AtomicBool::new(false), |
||||||
|
update_impl: Mutex::new(update_impl), |
||||||
|
state: Mutex::new(T::State::default()), |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub fn new_ref(update_impl: T) -> UpdatableRef { |
||||||
|
Arc::new(Self::new(update_impl)) |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<T: GenericUpdate> Updatable for GenericUpdatable<T> { |
||||||
|
fn update(&self, dt: f64) { |
||||||
|
if self.enabled.load(Ordering::SeqCst) { |
||||||
|
let (mut imp, mut state) = match (self.update_impl.lock(), self.state.lock()) { |
||||||
|
(Ok(a), Ok(b)) => (a, b), |
||||||
|
_ => return, |
||||||
|
}; |
||||||
|
|
||||||
|
imp.update(&mut *state, dt); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub trait ControllerInput { |
||||||
|
fn controller_input(&self) -> f64; |
||||||
|
} |
||||||
|
|
||||||
|
pub trait ControllerOutput { |
||||||
|
fn controller_output(&self, output: f64); |
||||||
|
} |
||||||
|
|
||||||
|
pub struct NullInputOutput; |
||||||
|
|
||||||
|
impl ControllerInput for NullInputOutput { |
||||||
|
fn controller_input(&self) -> f64 { |
||||||
|
0.0 |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl ControllerOutput for NullInputOutput { |
||||||
|
fn controller_output(&self, output: f64) {} |
||||||
|
} |
||||||
|
|
||||||
|
impl<T> ControllerInput for T |
||||||
|
where |
||||||
|
T: Fn() -> f64, |
||||||
|
{ |
||||||
|
fn controller_input(&self) -> f64 { |
||||||
|
self() |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<T> ControllerOutput for T |
||||||
|
where |
||||||
|
T: Fn(f64) -> (), |
||||||
|
{ |
||||||
|
fn controller_output(&self, output: f64) { |
||||||
|
self(output); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
use crate::motor_controller::{MotorController, MotorControllerRef}; |
||||||
|
|
||||||
|
impl ControllerOutput for MotorController { |
||||||
|
fn controller_output(&self, output: f64) { |
||||||
|
self.set(output); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl ControllerOutput for MotorControllerRef { |
||||||
|
fn controller_output(&self, output: f64) { |
||||||
|
self.set(output); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub trait SISO_Compute { |
||||||
|
type State: Default; |
||||||
|
|
||||||
|
fn compute(&mut self, &mut SISO_State<Self::State>, dt: f64) -> f64; |
||||||
|
} |
||||||
|
|
||||||
|
impl Default for Box<dyn ControllerInput> { |
||||||
|
fn default() -> Self { |
||||||
|
Box::new(NullInputOutput) |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl Default for Box<dyn ControllerOutput> { |
||||||
|
fn default() -> Self { |
||||||
|
Box::new(NullInputOutput) |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub struct SISO_Controller<TInner: SISO_Compute> { |
||||||
|
pub input: Box<dyn ControllerInput>, |
||||||
|
pub output: Box<dyn ControllerOutput>, |
||||||
|
|
||||||
|
pub setpoint_min: f64, |
||||||
|
pub setpoint_max: f64, |
||||||
|
pub output_min: f64, |
||||||
|
pub output_max: f64, |
||||||
|
pub output_offset: f64, |
||||||
|
|
||||||
|
pub inner: TInner, |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> std::ops::Deref for SISO_Controller<TInner> |
||||||
|
where |
||||||
|
TInner: SISO_Compute, |
||||||
|
{ |
||||||
|
type Target = TInner; |
||||||
|
fn deref(&self) -> &TInner { |
||||||
|
&self.inner |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> std::ops::DerefMut for SISO_Controller<TInner> |
||||||
|
where |
||||||
|
TInner: SISO_Compute, |
||||||
|
{ |
||||||
|
fn deref_mut(&mut self) -> &mut TInner { |
||||||
|
&mut self.inner |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> Default for SISO_Controller<TInner> |
||||||
|
where |
||||||
|
TInner: SISO_Compute + Default, |
||||||
|
{ |
||||||
|
fn default() -> Self { |
||||||
|
Self { |
||||||
|
input: Default::default(), |
||||||
|
output: Default::default(), |
||||||
|
setpoint_min: std::f64::NEG_INFINITY, |
||||||
|
setpoint_max: std::f64::INFINITY, |
||||||
|
output_min: std::f64::NEG_INFINITY, |
||||||
|
output_max: std::f64::INFINITY, |
||||||
|
output_offset: 0.0, |
||||||
|
inner: Default::default(), |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub struct SISO_State<TInner> { |
||||||
|
setpoint: f64, |
||||||
|
input: f64, |
||||||
|
|
||||||
|
inner: TInner, |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> std::ops::Deref for SISO_State<TInner> { |
||||||
|
type Target = TInner; |
||||||
|
fn deref(&self) -> &TInner { |
||||||
|
&self.inner |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> std::ops::DerefMut for SISO_State<TInner> { |
||||||
|
fn deref_mut(&mut self) -> &mut TInner { |
||||||
|
&mut self.inner |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> Default for SISO_State<TInner> |
||||||
|
where |
||||||
|
TInner: Default, |
||||||
|
{ |
||||||
|
fn default() -> Self { |
||||||
|
Self { |
||||||
|
setpoint: 0.0, |
||||||
|
input: 0.0, |
||||||
|
inner: Default::default(), |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> SISO_State<TInner> { |
||||||
|
pub fn error(&self) -> f64 { |
||||||
|
self.input - self.setpoint |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl<TInner> GenericUpdate for SISO_Controller<TInner> |
||||||
|
where |
||||||
|
TInner: SISO_Compute + Default, |
||||||
|
{ |
||||||
|
type State = SISO_State<TInner::State>; |
||||||
|
|
||||||
|
fn enable(&mut self, state: &mut Self::State) {} |
||||||
|
fn disable(&mut self, state: &mut Self::State) {} |
||||||
|
fn update(&mut self, state: &mut Self::State, dt: f64) { |
||||||
|
state.input = self.input.controller_input(); |
||||||
|
let output = self.inner.compute(state, dt); |
||||||
|
self.output.controller_output(output); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub struct PID_State { |
||||||
|
integral: f64, |
||||||
|
last_error: f64, |
||||||
|
} |
||||||
|
|
||||||
|
impl Default for PID_State { |
||||||
|
fn default() -> Self { |
||||||
|
Self { |
||||||
|
integral: 0.0, |
||||||
|
last_error: std::f64::NAN, |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub struct PID_Computer { |
||||||
|
k_p: f64, |
||||||
|
k_i: f64, |
||||||
|
k_d: f64, |
||||||
|
k_f: f64, |
||||||
|
} |
||||||
|
|
||||||
|
impl Default for PID_Computer { |
||||||
|
fn default() -> Self { |
||||||
|
Self { |
||||||
|
k_p: 0.0, |
||||||
|
k_i: 0.0, |
||||||
|
k_d: 0.0, |
||||||
|
k_f: 0.0, |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
impl SISO_Compute for PID_Computer { |
||||||
|
type State = PID_State; |
||||||
|
|
||||||
|
fn compute(&mut self, state: &mut SISO_State<PID_State>, dt: f64) -> f64 { |
||||||
|
let err = state.input - state.setpoint; |
||||||
|
let mut output = err * self.k_p; |
||||||
|
state.integral += err * dt; |
||||||
|
output += self.k_i * state.integral; |
||||||
|
if !state.last_error.is_nan() { |
||||||
|
output += self.k_d * dt * (err - state.last_error); |
||||||
|
} |
||||||
|
state.last_error = err; |
||||||
|
output += self.k_f * state.setpoint; |
||||||
|
output |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub type PID_Controller = SISO_Controller<PID_Computer>; |
@ -1,41 +1,64 @@ |
|||||||
use std::cell::RefCell; |
use std::cell::RefCell; |
||||||
use std::rc::Rc; |
use std::rc::Rc; |
||||||
|
|
||||||
use motor_controller::MockMotorController; |
use controller::PID_Controller; |
||||||
use motor_controller::MotorControllerBox; |
use motor_controller::{MockMotorController, MotorControllerNewRef, MotorControllerRef}; |
||||||
use motor_controller::MotorControllerBoxed; |
|
||||||
|
|
||||||
pub struct Drive { |
pub struct Drive { |
||||||
left_motors: Vec<MotorControllerBox>, |
left_motors: Vec<MotorControllerRef>, |
||||||
right_motors: Vec<MotorControllerBox>, |
right_motors: Vec<MotorControllerRef>, |
||||||
|
left_controller: PID_Controller, |
||||||
|
right_controller: PID_Controller, |
||||||
} |
} |
||||||
|
|
||||||
pub type DriveRef = Rc<RefCell<Drive>>; |
pub type DriveRef = Rc<RefCell<Drive>>; |
||||||
|
|
||||||
fn create_motor_controllers<T: IntoIterator<Item = u16>>(ids: T) -> Vec<MotorControllerBox> { |
fn create_motor_controllers<T: IntoIterator<Item = u16>>(ids: T) -> Vec<MotorControllerRef> { |
||||||
ids.into_iter().map(MockMotorController::new_box).collect() |
ids.into_iter().map(MockMotorController::new_ref).collect() |
||||||
} |
} |
||||||
|
|
||||||
impl Drive { |
impl Drive { |
||||||
pub fn new() -> Self { |
pub fn new_ref() -> Rc<RefCell<Self>> { |
||||||
let left_motors = create_motor_controllers(0u16..4); |
let left_motors = create_motor_controllers(0u16..4); |
||||||
let right_motors = create_motor_controllers(4u16..8); |
let right_motors = create_motor_controllers(4u16..8); |
||||||
Drive { |
let mut left_controller = PID_Controller::default(); |
||||||
|
left_controller.input = Box::new(|| 0.5); |
||||||
|
let mut right_controller = PID_Controller::default(); |
||||||
|
right_controller.input = Box::new(|| 0.5); |
||||||
|
right_controller.output = Box::new(left_motors[0].clone()); |
||||||
|
let drive = Drive { |
||||||
left_motors, |
left_motors, |
||||||
right_motors, |
right_motors, |
||||||
} |
left_controller, |
||||||
} |
right_controller, |
||||||
|
}; |
||||||
|
let drive = Rc::new(RefCell::new(drive)); |
||||||
|
|
||||||
|
drive.borrow_mut().left_controller.output = { |
||||||
|
let drive = drive.clone(); |
||||||
|
Box::new(move |out| drive.borrow().set_left_power(out)) |
||||||
|
}; |
||||||
|
drive.borrow_mut().right_controller.output = { |
||||||
|
let drive = drive.clone(); |
||||||
|
Box::new(move |out| drive.borrow().set_right_power(out)) |
||||||
|
}; |
||||||
|
|
||||||
pub fn new_ref() -> DriveRef { |
drive |
||||||
Rc::new(RefCell::new(Drive::new())) |
|
||||||
} |
} |
||||||
|
|
||||||
pub fn drive_powers(&mut self, left_power: f64, right_power: f64) { |
pub fn set_left_power(&self, left_power: f64) { |
||||||
for motor in &mut self.left_motors { |
for motor in &self.left_motors { |
||||||
motor.set(left_power); |
motor.set(left_power); |
||||||
} |
} |
||||||
for motor in &mut self.right_motors { |
} |
||||||
|
pub fn set_right_power(&self, right_power: f64) { |
||||||
|
for motor in &self.right_motors { |
||||||
motor.set(right_power); |
motor.set(right_power); |
||||||
} |
} |
||||||
} |
} |
||||||
|
|
||||||
|
pub fn drive_powers(&self, left_power: f64, right_power: f64) { |
||||||
|
self.set_left_power(left_power); |
||||||
|
self.set_right_power(right_power); |
||||||
|
} |
||||||
} |
} |
||||||
|
Loading…
Reference in new issue