Implement auto setpoint update

red/src/jogger.rs

@@ -1,16 +1,40 @@
+use gilrs::Axis::*;
+use gilrs::EventType::*;
 use gilrs::Gilrs;
-use tokio::sync::{oneshot, Mutex};
-use std::time::Duration;
+use std::sync::Arc;
+use std::sync::Mutex;
 use std::thread::sleep;
+use std::time::Duration;
+use tokio::sync::{mpsc, oneshot};
+
+#[derive(Debug)]
+pub enum Axis {
+    X,
+    Y,
+    // The z axis is controlled by the analog triggers
+    ZPositive,
+    ZNegative,
+}
+
+#[derive(Debug)]
+pub enum ControllerEvent {
+    AxisPosition(Axis, f32),
+    TerminatePressed,
+}
 
 pub struct Jogger {
-    speed_setpoint: Mutex<(f64, f64, f64)>,
+    speed_setpoint: Mutex<(f32, f32, f32)>,
     terminator: oneshot::Sender<()>,
 }
 
 impl Jogger {
-    pub async fn new() -> Result<Self, gilrs::Error> {
+    pub async fn new() -> Result<Arc<Self>, gilrs::Error> {
         let (terminate_tx, mut terminate_rx) = oneshot::channel();
+        let (controller_tx, controller_rx) = mpsc::channel(8);
+        let res = Arc::new(Jogger {
+            speed_setpoint: Mutex::new((0.0, 0.0, 0.0)),
+            terminator: terminate_tx,
+        });
         tokio::task::spawn_blocking(move || {
             let mut gilrs = Gilrs::new().unwrap();
             for (_id, gamepad) in gilrs.gamepads() {
@@ -19,13 +43,63 @@ impl Jogger {
             while let Err(oneshot::error::TryRecvError::Empty) = terminate_rx.try_recv() {
                 sleep(Duration::from_millis(1));
                 if let Some(event) = gilrs.next_event() {
-                    println!("{}", event.id);
+                    Self::map_event(event).map(|event| controller_tx.blocking_send(event));
                 }
             }
         });
-        Ok(Jogger {
-            speed_setpoint: Mutex::new((0.0, 0.0, 0.0)),
-            terminator: terminate_tx,
-        })
+
+        tokio::spawn(Self::handle_events(res.clone(), controller_rx));
+
+        Ok(res)
+    }
+
+    pub fn speed_setpoint(&self) -> (f32, f32, f32){
+        *self.speed_setpoint.lock().unwrap()
+    }
+
+    async fn handle_events(self_arc: Arc<Self>, mut events_rx: mpsc::Receiver<ControllerEvent>) {
+        let mut z_positive = 0.0;
+        let mut z_negative = 0.0;
+        while let Some(event) = events_rx.recv().await {
+            match event {
+                ControllerEvent::TerminatePressed => break,
+                ControllerEvent::AxisPosition(axis, value) => {
+                    // I won't panic if you don't panic!
+                    let mut speed_setpoint = self_arc.speed_setpoint.lock().unwrap();
+                    match axis {
+                        Axis::X => speed_setpoint.0 = value,
+                        Axis::Y => speed_setpoint.1 = value,
+                        Axis::ZPositive => {
+                            z_positive = value;
+                            speed_setpoint.2 = z_positive-z_negative;
+                        }
+                        Axis::ZNegative => {
+                            z_negative = value;
+                            speed_setpoint.2 = z_positive-z_negative;
+                        }
+                    }
+                }
+            }
+        }
+        println!("Controller quit!")
+    }
+
+    fn map_event(event: gilrs::Event) -> Option<ControllerEvent> {
+        match event.event {
+            AxisChanged(LeftStickX, value, _) => {
+                Some(ControllerEvent::AxisPosition(Axis::X, value))
+            }
+            AxisChanged(LeftStickY, value, _) => {
+                Some(ControllerEvent::AxisPosition(Axis::Y, value))
+            }
+            AxisChanged(RightZ, value, _) => {
+                Some(ControllerEvent::AxisPosition(Axis::ZNegative, (1.0+value)/2.0))
+            }
+            AxisChanged(LeftZ, value, _) => {
+                Some(ControllerEvent::AxisPosition(Axis::ZPositive, (1.0+value)/2.0))
+            }
+            ButtonPressed(gilrs::Button::Start, _) => Some(ControllerEvent::TerminatePressed),
+            _ => None,
+        }
     }
 }

red/src/main.rs

@@ -1,6 +1,6 @@
 #![warn(rust_2018_idioms)]
 use futures::never::Never;
-use red::controller;
+use red::jogger;
 use red::printer::gcode::*;
 use red::printer::Printer;
 use std::time::Duration;
@@ -9,9 +9,11 @@ const DEFAULT_TTY: &str = "/dev/ttyUSB0";
 
 #[tokio::main]
 async fn main() -> Never {
-    let jogger = controller::Jogger::new().await;
+    let jogger = jogger::Jogger::new().await.unwrap();
     loop {
         tokio::time::sleep(Duration::from_secs(2)).await;
+        let setpoint = jogger.speed_setpoint();
+        println!("speed setpoint: {} {} {}", setpoint.0, setpoint.1, setpoint.2);
     }
     // let mut printer = Printer::connect(DEFAULT_TTY).await.unwrap();
     // printer