Finish roland events

kareszklub · Jul 28, 2023 · 21d74f6 · 21d74f6 · github-actions · Jul 28, 2023
1 parent a8f01e2
commit 21d74f6
Show file tree

Hide file tree

Showing 15 changed files with 354 additions and 160 deletions.
diff --git a/build.sh b/build.sh
@@ -34,4 +34,5 @@ cmd="cargo $command --release --target=$target --features=$f"
 
 echo $cmd
 
-podman run --rm -it --init -v $(pwd):/src docker.io/vbeni/rust-arm-musl $cmd
+$docker run --rm -it --init -v $(pwd):/src docker.io/vbeni/rust-arm-musl $cmd
+
diff --git a/client/examples/controller.rs b/client/examples/controller.rs
@@ -47,8 +47,13 @@ fn main() -> Result<()> {
                     Concrete::Pwm(c) => robot.cmd(c)?,
                     Concrete::Servo(c) => robot.cmd(c)?,
 
-                    Concrete::Subscribe(c) => robot.cmd(c)?,
-                    Concrete::Unsubscribe(c) => robot.cmd(c)?,
+                    Concrete::Subscribe(_) => {
+                        println!("Subscribe no supported");
+                    }
+                    Concrete::Unsubscribe(_) => {
+                        println!("Unsubscribe no supported");
+                    }
+
                     Concrete::Nop(c) => robot.cmd(c)?,
                     Concrete::GetUptime(c) => println!("{:?}", robot.cmd(c)?),
 

diff --git a/client/src/transports/tcp.rs b/client/src/transports/tcp.rs
@@ -94,7 +94,7 @@ impl Subscribable for Tcp {
 
         let id = *id_handle;
         let ev = Into::<event::ConcreteType>::into(ev);
-        let cmd: cmd::Concrete = cmd::Subscribe(ev.clone()).into();
+        let cmd: cmd::Concrete = cmd::Subscribe(ev).into();
 
         let already_contains = handlers
             .insert(
@@ -118,7 +118,7 @@ impl Subscribable for Tcp {
 
     fn unsubscribe<E: roblib::event::Event>(&self, ev: E) -> anyhow::Result<()> {
         let concrete_event = ev.into();
-        let cmd: cmd::Concrete = cmd::Unsubscribe(concrete_event.clone()).into();
+        let cmd: cmd::Concrete = cmd::Unsubscribe(concrete_event).into();
 
         bincode::serialize_into(&self.socket, &cmd)?;
 

diff --git a/client/src/transports/udp.rs b/client/src/transports/udp.rs
@@ -2,7 +2,7 @@ use crate::Transport;
 use anyhow::Result;
 use roblib::{
     cmd::{self, has_return, Command},
-    event::{self, Event},
+    event::Event,
 };
 use serde::Deserialize;
 use std::{collections::HashMap, io::Cursor, sync::Arc};
@@ -70,18 +70,12 @@ impl Udp {
         }
         Ok(())
     }
-}
 
-impl Transport for Udp {
-    fn cmd<C>(&self, cmd: C) -> Result<C::Return>
+    fn cmd_id<C>(&self, cmd: C, id: u32) -> Result<C::Return>
     where
         C: Command,
         C::Return: Send + 'static,
     {
-        let mut id_handle = self.id.lock().unwrap();
-        let id = *id_handle;
-        *id_handle = id + 1;
-
         let concrete: cmd::Concrete = cmd.into();
         self.sock.send(&bincode::serialize(&(id, concrete))?)?;
 
@@ -107,6 +101,21 @@ impl Transport for Udp {
     }
 }
 
+impl Transport for Udp {
+    fn cmd<C>(&self, cmd: C) -> Result<C::Return>
+    where
+        C: Command,
+        C::Return: Send + 'static,
+    {
+        let mut id_handle = self.id.lock().unwrap();
+        let id = *id_handle;
+        *id_handle = id + 1;
+        drop(id_handle);
+
+        self.cmd_id(cmd, id)
+    }
+}
+
 impl Subscribable for Udp {
     fn subscribe<E, F>(&self, ev: E, mut handler: F) -> Result<()>
     where
@@ -115,13 +124,11 @@ impl Subscribable for Udp {
     {
         let mut id_handle = self.id.lock().unwrap();
         let id = *id_handle;
-        *id_handle += id + 1;
-
-        let ev = Into::<event::ConcreteType>::into(ev);
-        let cmd: cmd::Concrete = cmd::Subscribe(ev).into();
+        *id_handle = id + 1;
+        drop(id_handle);
 
-        let buf = bincode::serialize(&(id, cmd))?;
-        self.sock.send(&buf)?;
+        self.cmd_id(cmd::Subscribe(ev.into()), id)?
+            .map_err(anyhow::Error::msg)?;
 
         self.inner.handlers.lock().unwrap().insert(
             id,
@@ -133,7 +140,7 @@ impl Subscribable for Udp {
 
     fn unsubscribe<E: roblib::event::Event>(&self, ev: E) -> Result<()> {
         let ev = ev.into();
-        let cmd: cmd::Concrete = cmd::Unsubscribe(ev.clone()).into();
+        let cmd: cmd::Concrete = cmd::Unsubscribe(ev).into();
 
         self.sock.send(&bincode::serialize(&cmd)?)?;
 

diff --git a/roblib/src/camloc/event.rs b/roblib/src/camloc/event.rs
@@ -4,25 +4,25 @@ use roblib_macro::Event;
 use serde::{Deserialize, Serialize};
 use std::net::SocketAddr;
 
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct CamlocConnect;
 impl crate::event::Event for CamlocConnect {
     type Item = (SocketAddr, PlacedCamera);
 }
 
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct CamlocDisconnect;
 impl crate::event::Event for CamlocDisconnect {
     type Item = SocketAddr;
 }
 
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct CamlocPosition;
 impl crate::event::Event for CamlocPosition {
     type Item = Position;
 }
 
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct CamlocInfoUpdate;
 impl crate::event::Event for CamlocInfoUpdate {
     type Item = (SocketAddr, PlacedCamera);

diff --git a/roblib/src/cmd/mod.rs b/roblib/src/cmd/mod.rs
@@ -4,6 +4,7 @@ use serde::{de::DeserializeOwned, Serialize};
 pub mod concrete;
 
 use crate::event;
+
 #[cfg(feature = "roland")]
 pub use crate::roland::cmd::*;
 
@@ -30,13 +31,13 @@ pub const fn has_return<C: Command>() -> bool {
 pub struct Subscribe(pub event::ConcreteType);
 impl Command for Subscribe {
     const PREFIX: char = '+';
-    type Return = ();
+    type Return = Result<(), String>;
 }
 #[derive(Command, serde::Serialize, serde::Deserialize)]
 pub struct Unsubscribe(pub event::ConcreteType);
 impl Command for Unsubscribe {
     const PREFIX: char = '-';
-    type Return = ();
+    type Return = Result<(), String>;
 }
 
 #[derive(Command, serde::Serialize, serde::Deserialize)]

diff --git a/roblib/src/event.rs b/roblib/src/event.rs
@@ -1,10 +1,19 @@
 use serde::{de::DeserializeOwned, Deserialize, Serialize};
 
+#[cfg(feature = "roland")]
+pub use crate::roland::event::*;
+
+#[cfg(feature = "gpio")]
+pub use crate::gpio::event::*;
+
+#[cfg(feature = "camloc")]
+pub use crate::camloc::event::*;
+
 pub trait Event: Serialize + DeserializeOwned + Into<ConcreteType> + From<ConcreteType> {
     type Item: Serialize + DeserializeOwned;
 }
 
-#[derive(Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub enum ConcreteType {
     #[cfg(feature = "roland")]
     TrackSensor(crate::roland::event::TrackSensor),

diff --git a/roblib/src/gpio/backend/simple.rs b/roblib/src/gpio/backend/simple.rs
@@ -1,9 +1,6 @@
 use crate::{
     get_servo_pwm_durations,
-    gpio::{
-        event::{Event, Subscriber},
-        Mode,
-    },
+    gpio::{event::Event, Mode},
 };
 use rppal::gpio::{InputPin, OutputPin, Trigger};
 use std::{
@@ -14,6 +11,10 @@ use std::{
     },
 };
 
+pub trait Subscriber: Send + Sync {
+    fn handle(&self, event: Event);
+}
+
 enum Pin {
     Input(InputPin),
     Output(OutputPin),
@@ -98,14 +99,15 @@ impl SimpleGpioBackend {
                             return log::error!("Handlers removed without clearing interrupt!");
                         };
                         // rising/falling edge thing seems to be backwards
-                        let value = !(l as u8 != 0);
-                        if let Ok(_) = handle.last_value.fetch_update(SeqCst, SeqCst, |last| {
+                        let value = l as u8 == 0;
+                        let res = &handle.last_value.fetch_update(SeqCst, SeqCst, |last| {
                             log::debug!("({last}) {value}");
                             if value != last {
                                 return Some(value);
                             }
                             None
-                        }) {
+                        });
+                        if res.is_ok() {
                             let ev = Event::PinChanged(pin, value);
                             handle.handler.handle(ev)
                         }

diff --git a/roblib/src/gpio/event.rs b/roblib/src/gpio/event.rs
@@ -6,11 +6,7 @@ pub enum Event {
     PinChanged(u8, bool),
 }
 
-pub trait Subscriber: Send + Sync {
-    fn handle(&self, event: Event);
-}
-
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct GpioPin(pub u8);
 impl crate::event::Event for GpioPin {
     type Item = bool;

diff --git a/roblib/src/roland/backend.rs b/roblib/src/roland/backend.rs
@@ -2,7 +2,10 @@ use super::Roland;
 use crate::get_servo_pwm_durations;
 use anyhow::Result;
 use rppal::gpio::{Gpio, InputPin, OutputPin};
-use std::{sync::Mutex, time::Instant};
+use std::{
+    sync::Mutex,
+    time::{Duration, Instant},
+};
 
 pub mod constants {
     pub mod motors {
@@ -147,6 +150,8 @@ pub struct RolandBackend {
     servo: Mutex<Servo>,
     motor: Mutex<Motors>,
     leds: Mutex<Leds>,
+
+    gpio: Gpio,
 }
 
 impl Drop for RolandBackend {
@@ -168,13 +173,49 @@ impl RolandBackend {
 
             ultra_sensor: UltraSensor::new(&gpio)?.into(),
             track_sensor: TrackSensor::new(&gpio)?.into(),
+
+            gpio,
         };
 
         // ran here as well to reset servo to center
         roland.cleanup()?;
 
         Ok(roland)
     }
+
+    pub fn setup_tracksensor_interrupts(&self) -> Result<()> {
+        let mut s = self.track_sensor.lock().unwrap();
+
+        s.l1.set_interrupt(rppal::gpio::Trigger::Both)?;
+        s.l2.set_interrupt(rppal::gpio::Trigger::Both)?;
+        s.r1.set_interrupt(rppal::gpio::Trigger::Both)?;
+        s.r2.set_interrupt(rppal::gpio::Trigger::Both)?;
+
+        Ok(())
+    }
+
+    pub fn poll_tracksensor(&self, timeout: Option<Duration>) -> Result<Option<(usize, bool)>> {
+        let s = self.track_sensor.lock().unwrap();
+
+        let ps = [&s.l1, &s.l2, &s.r1, &s.r2];
+        let res = self.gpio.poll_interrupts(&ps, false, timeout)?;
+
+        let Some((p, v)) = res else { return Ok(None) };
+
+        Ok(Some((
+            ps.iter().position(|a| *a == p).unwrap(),
+            v as u8 != 0,
+        )))
+    }
+
+    pub fn clear_tracksensor_interrupts(&self) -> Result<()> {
+        let mut s = self.track_sensor.lock().unwrap();
+        s.l1.clear_interrupt()?;
+        s.l2.clear_interrupt()?;
+        s.r1.clear_interrupt()?;
+        s.r2.clear_interrupt()?;
+        Ok(())
+    }
 }
 
 impl Roland for RolandBackend {

diff --git a/roblib/src/roland/event.rs b/roblib/src/roland/event.rs
@@ -3,13 +3,13 @@ use std::time::Duration;
 use roblib_macro::Event;
 use serde::{Deserialize, Serialize};
 
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct TrackSensor;
 impl crate::event::Event for TrackSensor {
     type Item = [bool; 4];
 }
 
-#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Debug)]
+#[derive(Event, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy, Debug)]
 pub struct UltraSensor(pub Duration);
 impl crate::event::Event for UltraSensor {
     type Item = f64;
	base	roland
aarch64-unknown-linux-gnu	Download	Download
armv7-unknown-linux-gnueabihf	Download	Download
x86_64-pc-windows-msvc	Download	Download
x86_64-unknown-linux-gnu	Download	Download