logic.rs

use crate::{
    encoders::Encoders,
    tb6612fng::{Movement, Tb6612fng},
};
use core::sync::atomic::{AtomicU32, Ordering};
use embassy_time::{Instant, Timer};
use heapless::Deque;
use i2c2_target::{I2C2, MESSAGE_SIZE};

struct State {
    i2c: I2C2,
    motors: Tb6612fng<'static>,
    encoders: Encoders<'static>,
    max_motor_percentage: u8,
    motor_speed: [u8; 2],
    standby: bool,
    /// Number of 1/10th of seconds
    watchdog_ticks: u8,
}

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,
            watchdog_ticks: 5,
        }
    }

    #[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;

/// Date of the next watchdog expiration. A u32 value can hold up to
/// 49 days which is more than enough. It is not stored in the state
/// to be able to be checked outside any critical section.
static WATCHDOG_EXPIRATION: AtomicU32 = AtomicU32::new(0);

/// Number of milliseconds since boot
#[allow(clippy::cast_possible_truncation)]
fn current_millis() -> u32 {
    Instant::now().as_millis() as u32
}

#[embassy_executor::task]
pub async fn start_i2c_target(
    i2c2: I2C2,
    mut motors: Tb6612fng<'static>,
    encoders: Encoders<'static>,
) {
    motors.standby_enter();
    unsafe {
        STATE = Some(State::new(i2c2, motors, encoders));
        STATE.as_mut().unwrap().i2c.start(&i2c_callback);
    }
    loop {
        Timer::after_millis(100).await;
        if current_millis() >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed) {
            State::with_critical_section(|state| {
                if state.is_moving()
                    && current_millis() >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed)
                {
                    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_EXPIRATION.store(
            current_millis() + 100 * u32::from(state.watchdog_ticks),
            Ordering::Relaxed,
        );
    } else {
        state.standby();
    }
}

const FIRMWARE_VERSION: u8 = 0x08;
const WHO_AM_I: u8 = 0x0f;
const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
const MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
const MOTOR_SPEED: u8 = 0x30;
const ENCODER_TICKS: u8 = 0x32;
const 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 {
        [FIRMWARE_VERSION] => {
            for b in PKG_VERSION {
                response.push_back(b).unwrap();
            }
        }
        [WHO_AM_I, ..] => {
            response.push_back(0x57).unwrap();
        }
        &[MAX_MOTOR_PERCENTAGE, p] => {
            if (1..=100).contains(&p) {
                state.max_motor_percentage = p;
            } else {
                defmt::warn!("Incorrect max percentage {}", p);
                return false;
            }
        }
        [MAX_MOTOR_PERCENTAGE] => {
            response.push_back(state.max_motor_percentage).unwrap();
        }
        [MOTOR_SPEED, ms @ ..] if ms.len() == 2 => {
            state.set_motor_speed([ms[0], ms[1]]);
        }
        [MOTOR_SPEED] => {
            response.push_back(state.motor_speed[0]).unwrap();
            response.push_back(state.motor_speed[1]).unwrap();
        }
        &[MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
            if (1..=100).contains(&ticks) {
                state.watchdog_ticks = ticks;
            } else {
                defmt::warn!("ticks out of range: {}", ticks);
                return false;
            }
        }
        [MOTOR_SHUTDOWN_TIMEOUT] => {
            response.push_back(state.watchdog_ticks).unwrap();
        }
        [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();
        }
        [STATUS] => {
            response.push_back(u8::from(state.is_moving())).unwrap();
        }
        _ => {
            defmt::warn!("unknown command or args {:#04x}", command);
            return false;
        }
    }
    true
}