From 17553cf2a3b44e7379276a35fc0f5542dd76192e Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 4 Feb 2025 15:48:14 -0500
Subject: [PATCH] thingy drives

added deadzone to gamepad controller

mended drive polarity

wait to start telem until awakened by northbridge
(linux doesn't boot in 300 cycles)
---
 interface/src/main.rs   | 11 ++++++++---
 northbridge/src/main.rs | 26 +++++++++++++-------------
 southbridge/src/main.rs | 21 ++++++++++++++-------
 3 files changed, 35 insertions(+), 23 deletions(-)

diff --git a/interface/src/main.rs b/interface/src/main.rs
index 5b0fe62..b8b3095 100644
--- a/interface/src/main.rs
+++ b/interface/src/main.rs
@@ -24,7 +24,6 @@ async fn main() -> anyhow::Result<()> {
 
     let mut active_gamepad = None;
 
-    sleep(Duration::from_secs(5)).await; // bad
     loop {
         while let Some(Event { id,..}) = gamepads.next_event() {
             active_gamepad = Some(id);
@@ -41,8 +40,14 @@ async fn main() -> anyhow::Result<()> {
         let values = gamepad.state();
         dbg!(values);
 
-        let ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0);
-        let rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);
+        let mut ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0);
+        let mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);
+
+        for axis in [&mut rx, &mut ly] {
+            if axis.abs() < 0.05 {
+                *axis = 0.0;
+            }
+        }
 
         let cmd = Command::Twist(ly, rx);
 
diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs
index 7fe3a12..4ff4464 100644
--- a/northbridge/src/main.rs
+++ b/northbridge/src/main.rs
@@ -33,26 +33,26 @@ async fn main() -> Result<()> {
     write.flush().await?;
 
     loop {
-        //let Some(Ok(data)) = read.next().await else {
-        //    continue;
-        //};
-        //if data.enabled {
-        //    write.send(Command::FeedWatchdog).await?;
-        //    write.flush().await?;
+        let Some(Ok(data)) = read.next().await else {
+            continue;
+        };
+        if data.enabled {
+            write.send(Command::FeedWatchdog).await?;
+            write.flush().await?;
 
-        //} else {
-        //    println!("enabled southbridge");
-        //    write.send(Command::Enable).await?;
-        //}
+        } else {
+            println!("enabled southbridge");
+            write.send(Command::Enable).await?;
+        }
 
         write.send(Command::Enable).await?;
         write.flush().await?;
 
         //dbg!(&data);
 
-        //if let Some(data) = data.sensor_data {
-        //    let _ = sensor_sender.send(data);
-        //}
+        if let Some(data) = data.sensor_data {
+            let _ = sensor_sender.send(data);
+        }
         
         write.send(dbg!(commands.borrow().clone())).await?;
         write.flush().await?;
diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs
index bd24a09..3ccef24 100644
--- a/southbridge/src/main.rs
+++ b/southbridge/src/main.rs
@@ -6,7 +6,7 @@ use core::{panic::PanicInfo, sync::atomic::Ordering};
 use common::{Command, Response, SensorData, BAUDRATE};
 use embassy_executor::Spawner;
 use embassy_rp::{bind_interrupts, block::ImageDef, gpio::{Level, Output}, peripherals::{UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, BufferedUartTx, Config}, usb::Driver};
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
 use embedded_io_async::{BufRead, Read, Write};
 use log::{error, info, trace, warn};
@@ -51,8 +51,8 @@ async fn main(spawner: Spawner) {
 
     let mut drive_conf: pwm::Config = Default::default();
     drive_conf.divider = 150.into();
-    drive_conf.top = 20000; // 5ms
-    drive_conf.compare_b = 1000; // 1.5ms
+    drive_conf.top = 20000; // 20ms
+    drive_conf.compare_b = 1500; // 1.5ms
     drive_conf.compare_a = 1500; // 1.5ms
     let stopped = drive_conf.clone();
     let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, stopped.clone());
@@ -73,8 +73,10 @@ async fn main(spawner: Spawner) {
 
     spawner.spawn(decoder(rx)).unwrap();
 
+    static STARTUP: OnceLock<()> = OnceLock::new();
+
     static ENABLED: AtomicBool = AtomicBool::new(false);
-    spawner.spawn(telemetry(tx, &ENABLED)).unwrap();
+    spawner.spawn(telemetry(tx, &ENABLED, &STARTUP)).unwrap();
 
     let enabled = &ENABLED;
     let mut enable_watchdog = Instant::now();
@@ -97,10 +99,12 @@ async fn main(spawner: Spawner) {
             COMMANDS.receive().await
         };
 
+        STARTUP.get_or_init(||());
+
         match command {
             Command::Twist(forward, right) => {
-                drive_conf.compare_a = calc_speed(forward - right);
-                drive_conf.compare_b = calc_speed(forward + right);
+                drive_conf.compare_a = calc_speed(-forward - right);
+                drive_conf.compare_b = calc_speed(forward - right);
                 drive_pwm.set_config(&drive_conf);
             },
             Command::Stop => { 
@@ -150,7 +154,10 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
 
 /// Receive data from channel and send it over UART
 #[embassy_executor::task]
-async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool) {
+async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool, startup: &'static OnceLock<()>) {
+    info!("Telemetry waiting...");
+    startup.get().await;
+    info!("Telemetry started...");
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();