logic.rs

use crate::{
    encoders::Encoders,
    tb6612fng::{Movement, Tb6612fng},
};
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,
}

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

static mut STATE: Option<State> = None;

#[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(CALLBACK);
    }
}

const CALLBACK: i2c2_target::Callback = &handle_command;

const WHO_AM_I: u8 = 0x0f;
const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
const MOTOR_SPEED: u8 = 0x30;
const ENCODER_TICKS: u8 = 0x32;

fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
    #[allow(static_mut_refs)]
    let state = unsafe { STATE.as_mut().unwrap() };
    defmt::trace!("Processing command {:?}", command);
    match command {
        [WHO_AM_I, ..] => {
            let _ = response.push_back(0x57);
        }
        &[MAX_MOTOR_PERCENTAGE, p, ..] => {
            if (1..=100).contains(&p) {
                state.max_motor_percentage = p;
            } else {
                defmt::warn!("Incorrect max percentage {}", p);
            }
        }
        [MAX_MOTOR_PERCENTAGE] => {
            let _ = response.push_back(state.max_motor_percentage);
        }
        [MOTOR_SPEED, ms @ ..] if ms.len() >= 2 => {
            let (max_pwm, mmp) = (
                state.motors.max_pwm() as i32,
                i32::from(state.max_motor_percentage),
            );
            let scaled_speed = |speed| {
                (speed != 0x80).then(|| i32::from(speed as i8) * max_pwm * mmp / (127 * 100))
            };
            state.motor_speed = [ms[0], ms[1]];
            let (speed_left, speed_right) = (scaled_speed(ms[0]), scaled_speed(ms[1]));
            if (speed_left, speed_right) == (None, None) {
                state.motors.standby_enter();
                state.standby = true;
            } else {
                if state.standby {
                    state.motors.standby_leave();
                    state.standby = false;
                }
                if let Some(speed_left) = speed_left {
                    state.motors.move_left(Movement::Advance(speed_left));
                }
                if let Some(speed_right) = speed_right {
                    state.motors.move_right(Movement::Advance(speed_right));
                }
            }
        }
        [MOTOR_SPEED, ..] => {
            for &s in &state.motor_speed {
                let _ = response.push_back(s);
            }
        }
        [ENCODER_TICKS, ..] => {
            let (left, right) = state.encoders.ticks();
            let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
            let _ = response.push_back(left[0]);
            let _ = response.push_back(left[1]);
            let _ = response.push_back(right[0]);
            let _ = response.push_back(right[1]);
        }
        &[idx, ..] => {
            defmt::warn!("unknown register {:#04x}", idx);
        }
        &[] => {
            defmt::warn!("received empty command");
        }
    }
}