logic.rs

use crate::{
    encoders::Encoders,
    tb6612fng::{Movement, Tb6612fng},
};
use core::sync::atomic::{AtomicU8, Ordering};
use embassy_stm32::{
    pac::{self, wwdg::vals::Wdgtb},
    peripherals::WWDG,
    time::hz,
};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
use embassy_time::{Duration, Instant, Ticker};
use heapless::Deque;
use i2c2_target::{I2C2, MESSAGE_SIZE};

static WATCHDOG_TICKS: AtomicU8 = AtomicU8::new(5);

struct State {
    i2c: I2C2,
    motors: Tb6612fng<'static>,
    encoders: Encoders<'static>,
    max_motor_percentage: u8,
    motor_speed: [u8; 2],
    standby: bool,
}

impl State {
    fn new(i2c2: I2C2, motors: Tb6612fng<'static>, encoders: Encoders<'static>) -> Self {
        Self {
            i2c: i2c2,
            motors,
            encoders,
            max_motor_percentage: 100,
            motor_speed: [0, 0],
            standby: true,
        }
    }

    #[allow(clippy::cast_possible_wrap)]
    fn set_motor_speed(&mut self, ms: [u8; 2]) {
        let (max_pwm, mmp) = (
            self.motors.max_pwm() as i32,
            i32::from(self.max_motor_percentage),
        );
        let scaled_speed = |speed| match speed as i8 {
            -128 => None,
            speed @ (-100..=100) => Some(i32::from(speed) * max_pwm * mmp / (100 * 100)),
            speed => {
                defmt::warn!("speed {} out of range [-100, 100]", speed);
                None
            }
        };
        let (speed_left, speed_right) = (scaled_speed(ms[0]), scaled_speed(ms[1]));
        if (speed_left, speed_right) == (None, None) {
            self.motors.standby_enter();
            self.standby = true;
            self.motor_speed = [0, 0];
        } else {
            if self.standby {
                self.motors.standby_leave();
                self.standby = false;
            }
            if let Some(speed_left) = speed_left {
                self.motors.move_left(Movement::Advance(speed_left));
                self.motor_speed[0] = ms[0];
            }
            if let Some(speed_right) = speed_right {
                self.motors.move_right(Movement::Advance(speed_right));
                self.motor_speed[1] = ms[1];
            }
        }
    }

    fn standby(&mut self) {
        self.set_motor_speed([0x80, 0x80]);
    }

    fn is_moving(&self) -> bool {
        self.motor_speed != [0, 0]
    }

    fn with_critical_section<R, F: FnMut(&mut State) -> R>(mut f: F) -> R {
        let state = unsafe { STATE.as_mut().unwrap() };
        critical_section::with(|_| f(state))
    }
}

static mut STATE: Option<State> = None;

fn wwdg_configure() {
    // Enable WWDG clock
    pac::RCC.apb1enr().modify(|w| w.set_wwdgen(true));
    // With PCLK1 at 36MHz, 63 as a timeout value with wdgtb at /8
    // means ~58ms.  Comparing with a value of 45 means that at
    // least ~42ms must have passed before the watchdog can be
    // petted, so we have 8ms on each side.
    pac::WWDG.cfr().write(|w| {
        w.set_wdgtb(Wdgtb::DIV8);
        w.set_w((1 << 6) | 31);
    });
    wwdg_pet();
}

fn wwdg_pet() {
    // Reset watchdog and start it if not started yet
    pac::WWDG.cr().write(|w| {
        w.set_t((1 << 6) | 63);
        w.set_wdga(true);
    });
}

#[allow(clippy::used_underscore_binding)] // Clippy bug
#[embassy_executor::task]
pub async fn start_i2c_target(
    i2c2: I2C2,
    mut motors: Tb6612fng<'static>,
    encoders: Encoders<'static>,
    _wwdg: WWDG,
) {
    motors.standby_enter();
    unsafe {
        STATE = Some(State::new(i2c2, motors, encoders));
        STATE.as_mut().unwrap().i2c.start(&i2c_callback);
    }
    watchdog().await;
}

static WATCHDOG_PING: Signal<CriticalSectionRawMutex, Instant> = Signal::new();

async fn watchdog() {
    wwdg_configure();
    let mut ticker = Ticker::every(Duration::from_millis(50));
    let mut watchdog_expiration: Instant = Instant::now();
    loop {
        ticker.next().await;
        wwdg_pet();
        if let Some(ping) = WATCHDOG_PING.try_take() {
            watchdog_expiration = ping
                + Duration::from_millis(100 * u64::from(WATCHDOG_TICKS.load(Ordering::Relaxed)));
        }
        if Instant::now() >= watchdog_expiration {
            State::with_critical_section(|state| {
                if state.is_moving() {
                    defmt::debug!("stopping motors after watchdog has expired");
                    state.standby();
                }
            });
        }
    }
}

fn i2c_callback(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
    #[allow(static_mut_refs)]
    let state = unsafe { STATE.as_mut().unwrap() };
    if process_command(state, command, response) {
        WATCHDOG_PING.signal(Instant::now());
    } else {
        state.standby();
    }
}

const CMD_FIRMWARE_VERSION: u8 = 0x08;
const CMD_WHO_AM_I: u8 = 0x0f;
const CMD_PWM_FREQUENCY: u8 = 0x10;
const CMD_MAX_MOTOR_PERCENTAGE: u8 = 0x11;
const CMD_MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
const CMD_MOTOR_SPEED: u8 = 0x30;
const CMD_ENCODER_TICKS: u8 = 0x32;
const CMD_STATUS: u8 = 0x36;

include!(concat!(env!("OUT_DIR"), "/version.rs"));

fn process_command(
    state: &mut State,
    command: &[u8],
    response: &mut Deque<u8, MESSAGE_SIZE>,
) -> bool {
    defmt::trace!("Processing command {:?}", command);
    match command {
        [CMD_FIRMWARE_VERSION] => {
            for b in PKG_VERSION {
                response.push_back(b).unwrap();
            }
        }
        [CMD_WHO_AM_I, ..] => {
            response.push_back(0x57).unwrap();
        }
        &[CMD_PWM_FREQUENCY, a, b, c] => {
            let f = u32::from_le_bytes([a, b, c, 0]);
            if (1..=100_000).contains(&f) {
                state.motors.set_frequency(hz(f));
            } else {
                defmt::warn!("incorrect PWM frequency {}", f);
                return false;
            }
        }
        [CMD_PWM_FREQUENCY] => {
            let freq = state.motors.get_frequency().0;
            for &b in &freq.to_le_bytes()[..3] {
                response.push_back(b).unwrap();
            }
        }
        &[CMD_MAX_MOTOR_PERCENTAGE, p] => {
            if (1..=100).contains(&p) {
                state.max_motor_percentage = p;
            } else {
                defmt::warn!("incorrect max percentage {}", p);
                return false;
            }
        }
        [CMD_MAX_MOTOR_PERCENTAGE] => {
            response.push_back(state.max_motor_percentage).unwrap();
        }
        [CMD_MOTOR_SPEED, ms @ ..] if ms.len() == 2 => {
            state.set_motor_speed([ms[0], ms[1]]);
        }
        [CMD_MOTOR_SPEED] => {
            response.push_back(state.motor_speed[0]).unwrap();
            response.push_back(state.motor_speed[1]).unwrap();
        }
        &[CMD_MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
            if (1..=100).contains(&ticks) {
                WATCHDOG_TICKS.store(ticks, Ordering::Relaxed);
            } else {
                defmt::warn!("ticks out of range: {}", ticks);
                return false;
            }
        }
        [CMD_MOTOR_SHUTDOWN_TIMEOUT] => {
            response
                .push_back(WATCHDOG_TICKS.load(Ordering::Relaxed))
                .unwrap();
        }
        [CMD_ENCODER_TICKS] => {
            let (left, right) = state.encoders.ticks();
            let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
            response.push_back(left[0]).unwrap();
            response.push_back(left[1]).unwrap();
            response.push_back(right[0]).unwrap();
            response.push_back(right[1]).unwrap();
        }
        [CMD_STATUS] => {
            response.push_back(u8::from(state.is_moving())).unwrap();
        }
        _ => {
            defmt::warn!("unknown command or args {:#04x}", command);
            return false;
        }
    }
    true
}