tele0592 1.2.0

use color_eyre::eyre::Context;
use std::{
    fmt::Write,
    thread,
    time::{Duration, Instant},
};
use tele0592::{Controller as _, Device};

const PERIOD: Duration = Duration::from_millis(20);
const STEPS: usize = 200;

const RAW: bool = false;

fn main() -> color_eyre::Result<()> {
    color_eyre::install()?;
    let mut controller = rppal::i2c::I2c::with_bus(8).with_context(|| "opening I²C bus")?;
    controller.reset()?;
    thread::sleep(Duration::from_millis(100));

    println!(
        "Firmware version: {:?}",
        controller.check_firmware_version()?
    );
    controller.set_motor_shutdown_timeout(Duration::from_secs(1))?;
    controller.set_raw_pid_coefficients(0x80, 0x03, 0x00)?;

    let mut relative = controller.new_relative()?;
    let mut csv = String::with_capacity(2 << 20);
    writeln!(
        csv,
        "{}command,left ticks,right ticks",
        if RAW { "raw " } else { "" }
    )?;
    let start = Instant::now();
    for speed in [
        10, 20, 30, 40, 50, 60, 70, 80, 0, -10, -20, -30, -40, -50, -60, -70, -80, 0,
    ] {
        if RAW {
            println!(
                "{:?}: Setting raw motor speeds to {speed}",
                Instant::elapsed(&start)
            );
            controller.set_raw_motor_speed(Some(speed), Some(speed))?;
        } else {
            println!(
                "{:?}: Setting motor speeds to {speed}",
                Instant::elapsed(&start)
            );
            controller.set_motor_speed(i16::from(speed), i16::from(speed))?;
        }
        for _ in 1..STEPS {
            thread::sleep(PERIOD);
            let (left, right) = controller.get_relative_encoder_ticks(&mut relative)?;
            writeln!(csv, "{speed},{left},{right}")?;
        }
        let counters = controller.get_counters()?;
        println!(
            "⇒ BTF: {}, unknown: {}, incorrect: {}, emergency stops: {}",
            counters.btf,
            counters.unknown_command,
            counters.incorrect_processing,
            counters.emergency_stops
        );
    }
    std::fs::write("encoders.csv", csv)?;
    Ok(())
}