Extract line following into its own function and Add calibration

Cargo.lock

@@ -210,6 +210,7 @@ dependencies = [
  "paste",
  "pid",
  "rusttype",
+ "thiserror",
 ]
 
 [[package]]
@@ -801,6 +802,26 @@ dependencies = [
  "unicode-ident",
 ]
 
+[[package]]
+name = "thiserror"
+version = "1.0.38"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6a9cd18aa97d5c45c6603caea1da6628790b37f7a34b6ca89522331c5180fed0"
+dependencies = [
+ "thiserror-impl",
+]
+
+[[package]]
+name = "thiserror-impl"
+version = "1.0.38"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1fb327af4685e4d03fa8cbcf1716380da910eeb2bb8be417e7f9fd3fb164f36f"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
 [[package]]
 name = "threadpool"
 version = "1.8.1"

Cargo.toml

@@ -10,6 +10,7 @@ imageproc = "0.23"
 rusttype = "0.9"
 paste = "1.0"
 pid = "4.0"
+thiserror = "1.0"
 
 [profile.release]
 lto = true

src/display.rs

@@ -20,6 +20,31 @@ pub enum Parameter {
     Speed,
 }
 
+pub fn draw_calibration(screen: &mut Screen, parameter: &str, value: impl Display, font: &Font) {
+    let smal_scale = Scale::uniform(screen.yres() as f32 / 4.0 - 8.0);
+    let big_scale = Scale::uniform(screen.yres() as f32 / 2.0 - 8.0);
+    let center = screen.xres() as i32 / 2;
+    clear(screen);
+    draw_centered_text(screen, center, 4, smal_scale, font, "kalibrieren:");
+    draw_centered_text(
+        screen,
+        center,
+        screen.yres() as i32 / 4 + 4,
+        smal_scale,
+        font,
+        parameter,
+    );
+    draw_centered_text(
+        screen,
+        center,
+        screen.yres() as i32 / 2 + 4,
+        big_scale,
+        font,
+        format!("{value:.2}").as_str(),
+    );
+    screen.update();
+}
+
 pub fn draw_settings(
     screen: &mut Screen,
     kp: impl Display,

src/main.rs

@@ -2,19 +2,21 @@ mod buttons;
 mod display;
 
 use buttons::SmartEv3Buttons;
-use display::{draw_cycles, font};
+use display::{draw_calibration, draw_cycles, draw_driving, draw_finished, font};
 use ev3dev_lang_rust::{
     motors::{LargeMotor, MotorPort},
     sensors::{ColorSensor, SensorPort},
-    Screen,
+    Ev3Error, Ev3Result, Screen,
 };
 use pid::Pid;
+use rusttype::Font;
 use std::{
     error::Error,
     hint,
     thread::sleep,
     time::{Duration, Instant},
 };
+use thiserror::Error;
 
 fn main() -> Result<(), Box<dyn Error>> {
     let left_motor = LargeMotor::get(MotorPort::OutB)?;
@@ -31,56 +33,115 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     sensor.set_mode_rgb_raw()?;
 
-    let mut count = 0;
+    let black = calibrate_gray(&mut screen, &sensor, &mut buttons, "Schwarz", &font)?;
-    let mut controller = Pid::new(66.0, f32::INFINITY);
+    let white = calibrate_gray(&mut screen, &sensor, &mut buttons, "Weiß", &font)?;
-    controller.p(0.1, f32::INFINITY);
+    let setpoint = (black + white) / 2.0;
-    controller.i(0.0, f32::INFINITY);
-    controller.d(0.0, f32::INFINITY);
-    left_motor.set_duty_cycle_sp(0)?;
-    right_motor.set_duty_cycle_sp(0)?;
-    left_motor.run_direct()?;
-    right_motor.run_direct()?;
-    buttons.process();
-    let start = Instant::now();
-    let mut next = start;
-    while start.elapsed() < Duration::from_secs(20) && !buttons.is_enter_pressed() {
-        let color = sensor.get_rgb()?;
-        let measurement = gray(color);
-        let controll = controller.next_control_output(measurement).output;
-        left_motor.set_duty_cycle_sp((20 + (controll as i32)).clamp(-100, 100))?;
-        right_motor.set_duty_cycle_sp((20 - (controll as i32)).clamp(-100, 100))?;
-        count += 1;
-        next += Duration::from_millis(2);
-        buttons.process();
-        sleep_until(next);
-    }
-    left_motor.stop().and(right_motor.stop())?;
-    buttons.process();
 
-    draw_cycles(&mut screen, count, &font);
+    draw_driving(&mut screen, &font);
+
+    let result = follow_line(
+        (&left_motor, &right_motor),
+        &sensor,
+        &mut buttons,
+        setpoint,
+        (0.3, 0.0, 0.0),
+        20,
+    );
+    left_motor.stop().and(right_motor.stop())?;
+    let time = match result {
+        Ok(time) => time,
+        Err(LineFollowError::UserAbort) => return Ok(()),
+        e => e?,
+    };
+
+    buttons.process();
+    draw_finished(&mut screen, time, &font);
     while !buttons.is_enter_pressed() {
         buttons.process();
     }
 
-    left_motor.stop().and(right_motor.stop())?;
     Ok(())
 }
 
+fn calibrate_gray(
+    screen: &mut Screen,
+    sensor: &ColorSensor,
+    buttons: &mut SmartEv3Buttons,
+    name: &str,
+    font: &Font,
+) -> Ev3Result<f32> {
+    loop {
+        let measurement = gray(sensor.get_rgb()?);
+        draw_calibration(screen, name, measurement, font);
+        buttons.process();
+        if buttons.is_enter_pressed() {
+            break Ok(measurement);
+        }
+    }
+}
+
+#[derive(Error, Debug)]
+enum LineFollowError {
+    #[error("working with EV3 peripheral")]
+    Ev3(#[from] Ev3Error),
+    #[error("the user abortet the run")]
+    UserAbort,
+}
+
+enum LineFollowState {
+    PreStart,
+    Start(Instant),
+    Run(Instant),
+    Finish(Duration),
+}
+
 fn follow_line(
-    (left, right): (LargeMotor, LargeMotor),
+    (left, right): (&LargeMotor, &LargeMotor),
-    sensor: ColorSensor,
+    sensor: &ColorSensor,
-    buttons: SmartEv3Buttons,
+    buttons: &mut SmartEv3Buttons,
     setpoint: f32,
-    k_p: f32,
+    (k_p, k_i, k_d): (f32, f32, f32),
-    k_i: f32,
+    v: i32,
-    k_d: f32,
+) -> Result<Duration, LineFollowError> {
-    v: f32,
+    let mut controller = Pid::new(setpoint, f32::INFINITY);
-) -> Result<Duration, Box<dyn Error>> {
-    let controller = Pid::new(setpoint, f32::INFINITY);
     controller
         .p(k_p, f32::INFINITY)
         .i(k_i, f32::INFINITY)
         .d(k_d, f32::INFINITY);
+    left.set_duty_cycle_sp(0)?;
+    right.set_duty_cycle_sp(0)?;
+    buttons.process();
+    let mut state = LineFollowState::PreStart;
+
+    left.run_direct()?;
+    right.run_direct()?;
+    let mut next = Instant::now();
+    while !(buttons.is_enter_pressed() || matches!(state, LineFollowState::Finish(_))) {
+        let color = sensor.get_rgb()?;
+        let measurement = gray(color);
+        let controll = controller.next_control_output(measurement).output;
+        left.set_duty_cycle_sp((v + (controll as i32)).clamp(-100, 100))?;
+        right.set_duty_cycle_sp((v - (controll as i32)).clamp(-100, 100))?;
+        let red = is_red(color);
+        match state {
+            LineFollowState::PreStart if red => state = LineFollowState::Start(Instant::now()),
+            LineFollowState::Start(start) if !red => state = LineFollowState::Run(start),
+            LineFollowState::Run(start) if red => state = LineFollowState::Finish(start.elapsed()),
+            _ => (),
+        }
+        buttons.process();
+        // 500Hz update rate
+        next += Duration::from_millis(2);
+        sleep_until(next);
+    }
+
+    left.stop()?;
+    right.stop()?;
+    if let LineFollowState::Finish(time) = state {
+        Ok(time)
+    } else {
+        Err(LineFollowError::UserAbort)
+    }
 }
 
 fn sleep_until(time: Instant) {
@@ -91,6 +152,12 @@ fn gray((r, g, b): (i32, i32, i32)) -> f32 {
     (r + g + b) as f32 / 3.0
 }
 
+const fn is_red((r, g, b): (i32, i32, i32)) -> bool {
+    let threshold = r / 2;
+    // check for not black and green and blue significantly lower than red
+    r > 32 && g < threshold && b < threshold
+}
+
 /*
 use std::{
     sync::{