use color_eyre::eyre::Context;
use std::time::{Duration, Instant};
use tele0592::Controller as _;
fn main() -> color_eyre::Result<()> {
color_eyre::install()?;
let mut controller = rppal::i2c::I2c::with_bus(8).with_context(|| "opening I²C bus")?;
println!(
"Firmware version: {:?}",
controller.check_firmware_version()?
);
controller.set_motor_shutdown_timeout(Duration::from_secs(3))?;
let timeout = controller.get_motor_shutdown_timeout()?;
println!("Motor shutdown timeout set to {timeout:?}");
println!("Reading encoders");
let mut relative = controller.new_relative()?;
println!("Making motors move, tracing ticks for 2s");
controller.set_motor_speed(20, -20)?;
let now = Instant::now();
while now.elapsed().as_secs() < 2 {
println!(
"Encoders = {:?}",
controller.get_relative_encoder_ticks(&mut relative)?
);
std::thread::sleep(Duration::from_millis(100));
}
println!("Motors should shutdown in {timeout:?}");
Ok(())
}