From 9b1b289b924f6c7a8afc8c87f1538b239d3bba20 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Sun, 19 Jan 2025 04:50:44 -0500
Subject: [PATCH] don't wait to stop charging, debouncing handled by user

---
 controller/src/main.rs | 27 +++++++++++----------------
 1 file changed, 11 insertions(+), 16 deletions(-)

diff --git a/controller/src/main.rs b/controller/src/main.rs
index 7ce17cc..830886c 100644
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@@ -88,7 +88,7 @@ async fn main(spawner: Spawner) {
     let driver = Driver::new(p.USB, Irqs);
     spawner.spawn(logger_task(driver)).unwrap();
 
-    let mut limit_switch = Input::new(p.PIN_22, Pull::Up);
+    let limit_switch = Input::new(p.PIN_22, Pull::Up);
 
     let mut d: pwm::Config = Default::default();
     d.divider = 40.into();
@@ -247,8 +247,8 @@ async fn main(spawner: Spawner) {
                         info!("left to {}", clamp(forward+right, -1., 1.));
                         info!("right to {}", clamp(forward-right, -1., 1.));
 
-                        c.compare_a = calc_speed(right + forward);
-                        c.compare_b = calc_speed(right - forward);
+                        c.compare_a = calc_speed(forward - right);
+                        c.compare_b = calc_speed(forward + right);
                         drive_pwm.set_config(&c);
                     },
                     ControlPacket::Fire => {
@@ -266,7 +266,7 @@ async fn main(spawner: Spawner) {
                     },
                     ControlPacket::Arm(enable) => {
                         if enable {
-                            cam_state.replace(CamState::Charging(None));
+                            cam_state.replace(CamState::Charging);
                         } else {
                             cam_state.replace(CamState::Idle);
                         }
@@ -302,9 +302,10 @@ fn calc_speed(speed: f32) -> u16 {
     (COUNTS_PER_MS * ms) as u16
 }
 
+#[derive(Debug)]
 enum CamState {
     Override(f32),
-    Charging(Option<Instant>),
+    Charging,
     Charged,
     Fire,
     Idle,
@@ -314,7 +315,7 @@ impl CamState {
     fn to_telem(&self) -> common::CamState {
         match self {
             Self::Override(_) | Self::Fire => common::CamState::Firing,
-            Self::Charging(_) => common::CamState::Charging,
+            Self::Charging => common::CamState::Charging,
             Self::Charged => common::CamState::Charged,
             Self::Idle => common::CamState::Idle,
         }
@@ -337,23 +338,17 @@ fn handle_cam(cam: &mut Pwm, state: &mut CamState, limit: &Input) {
         CamState::Override(speed) => {
             *speed
         },
-        CamState::Charging(mut charged) => {
+        CamState::Charging => {
             if limit.is_low() {
-                let became_low = charged.get_or_insert_with(Instant::now);
-                if became_low.elapsed().as_micros() > 1800 {
-                    *state = CamState::Charged;
-                    0.0
-                } else {
-                    1.0
-                }
+                *state = CamState::Charged;
+                0.0
             } else {
-                charged = None;
                 1.0
             }
         },
         CamState::Fire => {
             if limit.is_high() {
-                *state = CamState::Charging(None);
+                *state = CamState::Charging;
             }
             1.0
         },