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), + 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 },