some controllet stuff

src/controller.rs

@@ -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>;

src/drive.rs

@@ -1,41 +1,64 @@
 use std::cell::RefCell;
 use std::rc::Rc;
 
-use motor_controller::MockMotorController;
-use motor_controller::MotorControllerBox;
-use motor_controller::MotorControllerBoxed;
+use controller::PID_Controller;
+use motor_controller::{MockMotorController, MotorControllerNewRef, MotorControllerRef};
 
 pub struct Drive {
-    left_motors: Vec<MotorControllerBox>,
-    right_motors: Vec<MotorControllerBox>,
+    left_motors: Vec<MotorControllerRef>,
+    right_motors: Vec<MotorControllerRef>,
+    left_controller: PID_Controller,
+    right_controller: PID_Controller,
 }
 
 pub type DriveRef = Rc<RefCell<Drive>>;
 
-fn create_motor_controllers<T: IntoIterator<Item = u16>>(ids: T) -> Vec<MotorControllerBox> {
-    ids.into_iter().map(MockMotorController::new_box).collect()
+fn create_motor_controllers<T: IntoIterator<Item = u16>>(ids: T) -> Vec<MotorControllerRef> {
+    ids.into_iter().map(MockMotorController::new_ref).collect()
 }
 
 impl Drive {
-    pub fn new() -> Self {
+    pub fn new_ref() -> Rc<RefCell<Self>> {
         let left_motors = create_motor_controllers(0u16..4);
         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,
             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 {
-        Rc::new(RefCell::new(Drive::new()))
+        drive
     }
 
-    pub fn drive_powers(&mut self, left_power: f64, right_power: f64) {
-        for motor in &mut self.left_motors {
+    pub fn set_left_power(&self, left_power: f64) {
+        for motor in &self.left_motors {
             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);
         }
     }
+
+    pub fn drive_powers(&self, left_power: f64, right_power: f64) {
+        self.set_left_power(left_power);
+        self.set_right_power(right_power);
+    }
 }

src/main.rs

@@ -1,4 +1,6 @@
 #![feature(drain_filter)]
+#![feature(integer_atomics)]
+#![feature(duration_float)]
 
 #[macro_use]
 extern crate log;
@@ -9,6 +11,7 @@ mod command;
 mod drive;
 mod motor_controller;
 mod robot;
+mod controller;
 
 use robot::Robot;
 

src/motor_controller.rs

@@ -1,6 +1,9 @@
+use std::sync::Arc;
+use std::cell::UnsafeCell;
+
 pub trait MotorController {
     fn id(&self) -> u16;
-    fn set(&mut self, value: f64);
+    fn set(&self, value: f64);
     fn get(&self) -> f64;
 }
 
@@ -8,26 +11,26 @@ pub trait MotorControllerId: MotorController {
     fn new(id: u16) -> Self;
 }
 
-pub type MotorControllerBox = Box<dyn MotorController>;
+pub type MotorControllerRef = Arc<dyn MotorController>;
 
-pub trait MotorControllerBoxed {
-    fn new_box(id: u16) -> MotorControllerBox;
+pub trait MotorControllerNewRef {
+    fn new_ref(id: u16) -> MotorControllerRef;
 }
 
-impl<T: MotorControllerId + 'static> MotorControllerBoxed for T {
-    fn new_box(id: u16) -> MotorControllerBox {
-        Box::new(T::new(id))
+impl<T: MotorControllerId + 'static> MotorControllerNewRef for T {
+    fn new_ref(id: u16) -> MotorControllerRef {
+        Arc::new(T::new(id))
     }
 }
 
 pub struct MockMotorController {
     id: u16,
-    value: f64,
+    value: UnsafeCell<f64>, // meh
 }
 
 impl MotorControllerId for MockMotorController {
     fn new(id: u16) -> Self {
-        MockMotorController { id, value: 0.0 }
+        MockMotorController { id, value: UnsafeCell::new(0.0) }
     }
 }
 
@@ -36,12 +39,12 @@ impl MotorController for MockMotorController {
         self.id
     }
 
-    fn set(&mut self, value: f64) {
-        self.value = value;
-        debug!("MotorController id {} value={}", self.id, self.value);
+    fn set(&self, value: f64) {
+        unsafe { *self.value.get() = value; }
+        debug!("MotorController id {} value={}", self.id, value);
     }
 
     fn get(&self) -> f64 {
-        self.value
+        unsafe { *self.value.get() }
     }
 }