single threaded approach

Cargo.lock

@@ -207,6 +207,8 @@ dependencies = [
  "ev3dev-lang-rust",
  "image",
  "imageproc",
+ "paste",
+ "pid",
  "rusttype",
 ]
 
@@ -586,6 +588,15 @@ version = "1.0.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d01a5bd0424d00070b0098dd17ebca6f961a959dead1dbcbbbc1d1cd8d3deeba"
 
+[[package]]
+name = "pid"
+version = "4.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d7c931ef9756cd5e3fa3d395bfe09df4dfa6f0612c6ca8f6b12927d17ca34e36"
+dependencies = [
+ "num-traits",
+]
+
 [[package]]
 name = "pin-project"
 version = "1.0.12"

Cargo.toml

@@ -8,6 +8,9 @@ ev3dev-lang-rust = { version = "0.12.0", features = ["screen"] }
 image = "0.24"
 imageproc = "0.23"
 rusttype = "0.9"
+paste = "1.0"
+pid = "4.0"
 
 [profile.release]
 lto = true
+strip = "debuginfo"

src/buttons.rs

@@ -0,0 +1,59 @@
+use ev3dev_lang_rust::Ev3Button;
+use ev3dev_lang_rust::Ev3Result;
+use paste::paste;
+
+macro_rules! button_impl {
+    ($($name: ident),*) => {
+        paste! {
+
+            pub struct SmartEv3Buttons {
+                buttons: Ev3Button,
+                $(
+                    [<last_ $name>]: bool,
+                )*
+            }
+
+            impl SmartEv3Buttons {
+                pub fn new() -> Ev3Result<Self> {
+                    let buttons = Ev3Button::new()?;
+                    buttons.process();
+                    $(
+                        let [<last_ $name>] = buttons.[<is_ $name>]();
+                    )*
+                        Ok(Self {
+                            buttons,
+                            $(
+                                [<last_ $name>],
+                            )*
+                        })
+                }
+
+                pub fn process(&mut self) {
+                    $(
+                        self.[<last_ $name>] = self.buttons.[<is_ $name>]();
+                    )*
+                    self.buttons.process();
+                }
+
+                $(
+                    #[allow(dead_code)]
+                    pub fn [<is_ $name>](&self) -> bool {
+                        self.buttons.[<is_ $name>]()
+                    }
+
+                    #[allow(dead_code)]
+                    pub fn [<is_ $name _pressed>](&self) -> bool {
+                        self.buttons.[<is_ $name>]() && !self.[<last_ $name>]
+                    }
+
+                    #[allow(dead_code)]
+                    pub fn [<is_ $name _released>](&self) -> bool {
+                        !self.buttons.[<is_ $name>]() && self.[<last_ $name>]
+                    }
+                )*
+            }
+        }
+    };
+}
+
+button_impl!(up, down, left, right, enter, backspace);

src/display.rs

@@ -0,0 +1,197 @@
+use std::{fmt::Display, time::Duration};
+
+use ev3dev_lang_rust::Screen;
+use image::Rgb;
+use imageproc::{
+    drawing::{draw_line_segment_mut, draw_polygon_mut, draw_text_mut, text_size},
+    point::Point,
+};
+use rusttype::{Font, Scale};
+
+const BACKGROUND: Rgb<u8> = Rgb([255; 3]);
+const FOREGROUND: Rgb<u8> = Rgb([0; 3]);
+const FONT_DATA: &[u8] = include_bytes!("../fonts/RobotoMono-Regular.ttf");
+
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+pub enum Parameter {
+    Kp,
+    Ki,
+    Kd,
+    Speed,
+}
+
+pub fn draw_settings(
+    screen: &mut Screen,
+    kp: impl Display,
+    ki: impl Display,
+    kd: impl Display,
+    v: impl Display,
+    selected: Parameter,
+    font: &Font,
+) {
+    let text_scale = Scale::uniform(screen.yres() as f32 / 4.0 - 8.0);
+    clear(screen);
+    draw_centered_text(
+        screen,
+        screen.xres() as i32 / 2,
+        4,
+        text_scale,
+        font,
+        "Settings",
+    );
+
+    draw_setting(screen, kp, selected == Parameter::Kp, 0, "k_p", font);
+    draw_setting(screen, ki, selected == Parameter::Ki, 1, "k_i", font);
+    draw_setting(screen, kd, selected == Parameter::Kd, 2, "k_d", font);
+    draw_setting(screen, v, selected == Parameter::Speed, 3, "v", font);
+
+    screen.update();
+}
+
+pub fn draw_driving(screen: &mut Screen, font: &Font) {
+    clear(screen);
+    draw_centered_text(
+        screen,
+        screen.xres() as i32 / 2,
+        4,
+        Scale::uniform(screen.yres() as f32 / 2.0 - 8.0),
+        font,
+        "Fahrt",
+    );
+    screen.update();
+}
+
+pub fn draw_finished(screen: &mut Screen, time: Duration, font: &Font) {
+    clear(screen);
+    let text_scale = Scale::uniform(screen.yres() as f32 / 2.0 - 8.0);
+    draw_centered_text(
+        screen,
+        screen.xres() as i32 / 2,
+        4,
+        text_scale,
+        font,
+        "Zeit:",
+    );
+    draw_centered_text(
+        screen,
+        screen.xres() as i32 / 2,
+        screen.yres() as i32 / 2 + 4,
+        text_scale,
+        font,
+        format!("{:.2}s", time.as_secs_f32()).as_str(),
+    );
+    screen.update();
+}
+
+pub fn draw_cycles(screen: &mut Screen, cycles: u32, font: &Font) {
+    clear(screen);
+    let text_scale = Scale::uniform(screen.yres() as f32 / 2.0 - 8.0);
+    draw_centered_text(
+        screen,
+        screen.xres() as i32 / 2,
+        4,
+        text_scale,
+        font,
+        "Cycles:",
+    );
+    draw_centered_text(
+        screen,
+        screen.xres() as i32 / 2,
+        screen.yres() as i32 / 2 + 4,
+        text_scale,
+        font,
+        format!("{cycles}").as_str(),
+    );
+    screen.update();
+}
+
+pub fn font() -> Font<'static> {
+    Font::try_from_bytes(FONT_DATA).unwrap()
+}
+
+fn draw_setting(
+    screen: &mut Screen,
+    value: impl Display,
+    selected: bool,
+    index: u8,
+    name: &str,
+    font: &Font,
+) {
+    let text_scale = Scale::uniform(screen.yres() as f32 / 4.0 - 8.0);
+    let x = screen.xres() as i32 / 8 + screen.xres() as i32 * i32::from(index) / 4;
+    draw_centered_text(
+        screen,
+        x,
+        screen.yres() as i32 / 4 + 4,
+        text_scale,
+        font,
+        name,
+    );
+    draw_centered_text(
+        screen,
+        x,
+        screen.yres() as i32 * 5 / 8 + 4,
+        text_scale,
+        font,
+        format!("{value}").as_str(),
+    );
+    let top_triangle = [
+        Point::new(x, screen.yres() as i32 / 2),
+        Point::new(x - 10, screen.yres() as i32 / 2 + 10),
+        Point::new(x + 10, screen.yres() as i32 / 2 + 10),
+    ];
+    let bottom_triangle = [
+        Point::new(x, screen.yres() as i32),
+        Point::new(x - 10, screen.yres() as i32 - 10),
+        Point::new(x + 10, screen.yres() as i32 - 10),
+    ];
+    if selected {
+        draw_polygon_mut(&mut screen.image, &top_triangle, FOREGROUND);
+        draw_polygon_mut(&mut screen.image, &bottom_triangle, FOREGROUND);
+    } else {
+        for (start, end) in [
+            (top_triangle[0], top_triangle[1]),
+            (top_triangle[1], top_triangle[2]),
+            (top_triangle[2], top_triangle[0]),
+            (bottom_triangle[0], bottom_triangle[1]),
+            (bottom_triangle[1], bottom_triangle[2]),
+            (bottom_triangle[2], bottom_triangle[0]),
+        ] {
+            draw_line_segment_mut(
+                &mut screen.image,
+                point2tuple(start),
+                point2tuple(end),
+                FOREGROUND,
+            );
+        }
+    }
+}
+
+fn draw_centered_text(
+    screen: &mut ev3dev_lang_rust::Screen,
+    x: i32,
+    y: i32,
+    scale: Scale,
+    font: &Font,
+    text: &str,
+) {
+    let (width, _) = text_size(scale, font, text);
+    draw_text_mut(
+        &mut screen.image,
+        FOREGROUND,
+        x - width / 2,
+        y,
+        scale,
+        font,
+        text,
+    );
+}
+
+fn clear(screen: &mut Screen) {
+    screen.image.fill(BACKGROUND.0[0]);
+}
+
+const fn point2tuple(point: Point<i32>) -> (f32, f32) {
+    #[allow(clippy::cast_precision_loss)]
+    (point.x as f32, point.y as f32)
+}

src/main.rs

@@ -1,82 +1,641 @@
+mod buttons;
+mod display;
+
+use buttons::SmartEv3Buttons;
+use display::{draw_cycles, font};
 use ev3dev_lang_rust::{
     motors::{LargeMotor, MotorPort},
     sensors::{ColorSensor, SensorPort},
-    Ev3Button, Ev3Result, Screen,
+    Screen,
 };
-use image::{Rgb, RgbImage};
+use pid::Pid;
+use std::{
+    error::Error,
+    hint,
+    thread::sleep,
+    time::{Duration, Instant},
+};
+
+fn main() -> Result<(), Box<dyn Error>> {
+    let left_motor = LargeMotor::get(MotorPort::OutB)?;
+    let right_motor = LargeMotor::get(MotorPort::OutC)?;
+    let sensor = ColorSensor::get(SensorPort::In3)?;
+    let mut buttons = SmartEv3Buttons::new()?;
+    let mut screen = Screen::new()?;
+    let font = font();
+
+    left_motor.set_polarity(LargeMotor::POLARITY_INVERSED)?;
+    right_motor.set_polarity(LargeMotor::POLARITY_INVERSED)?;
+    left_motor.set_stop_action(LargeMotor::STOP_ACTION_BRAKE)?;
+    right_motor.set_stop_action(LargeMotor::STOP_ACTION_BRAKE)?;
+
+    sensor.set_mode_rgb_raw()?;
+
+    let mut count = 0;
+    let mut controller = Pid::new(66.0, f32::INFINITY);
+    controller.p(0.1, f32::INFINITY);
+    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);
+    while !buttons.is_enter_pressed() {
+        buttons.process();
+    }
+
+    left_motor.stop().and(right_motor.stop())?;
+    Ok(())
+}
+
+fn follow_line(
+    (left, right): (LargeMotor, LargeMotor),
+    sensor: ColorSensor,
+    buttons: SmartEv3Buttons,
+    setpoint: f32,
+    k_p: f32,
+    k_i: f32,
+    k_d: f32,
+    v: f32,
+) -> 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);
+}
+
+fn sleep_until(time: Instant) {
+    sleep(time - Instant::now());
+}
+
+fn gray((r, g, b): (i32, i32, i32)) -> f32 {
+    (r + g + b) as f32 / 3.0
+}
+
+/*
+use std::{
+    sync::{
+        atomic::{AtomicBool, Ordering},
+        mpsc::{channel, Receiver, Sender},
+        Arc,
+    },
+    thread::{self, sleep},
+    time::{Duration, Instant},
+};
+
+use ev3dev_lang_rust::{
+    motors::{LargeMotor, MotorPort},
+    sensors::{ColorSensor, SensorPort},
+    Device, Ev3Button, Ev3Result, Screen,
+};
+use image::Rgb;
 use imageproc::{
-    drawing::{draw_hollow_rect_mut, draw_text_mut},
+    drawing::{
+        draw_filled_rect_mut, draw_line_segment_mut, draw_polygon_mut, draw_text_mut, text_size,
+    },
+    point::Point,
     rect::Rect,
 };
+use paste::paste;
+use pid::Pid;
 use rusttype::{Font, Scale};
 
 fn main() -> Ev3Result<()> {
     let left_motor = LargeMotor::get(MotorPort::OutB)?;
     let right_motor = LargeMotor::get(MotorPort::OutC)?;
 
-    let res = mainloop(&left_motor, &right_motor);
+    let stop = Arc::new(AtomicBool::new(false));
+    let stop_clone = stop.clone();
+    let reset = Arc::new(AtomicBool::new(false));
+    let reset_clone = reset.clone();
+    let (pid_tx, pid_rx) = channel();
+    let (duration_tx, duration_rx) = channel();
+
+    let ui = thread::spawn(move || {
+        ui_thread(&stop_clone, &reset_clone, &pid_tx, &duration_rx).ok();
+        stop_clone.store(true, Ordering::Relaxed);
+    });
+
+    let res = mainloop(
+        &left_motor,
+        &right_motor,
+        &stop,
+        &reset,
+        &pid_rx,
+        &duration_tx,
+    );
 
     // Make sure the motors stop
+    stop.store(true, Ordering::Relaxed);
     left_motor.stop().and(right_motor.stop())?;
+    ui.join().unwrap();
     res
 }
 
-fn mainloop(left_motor: &LargeMotor, right_motor: &LargeMotor) -> Ev3Result<()> {
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+enum DriveState {
+    Start,
+    RedLine,
+    Measuring,
+    Finish,
+}
+
+fn mainloop(
+    left_motor: &LargeMotor,
+    right_motor: &LargeMotor,
+    stop: &Arc<AtomicBool>,
+    reset: &Arc<AtomicBool>,
+    pid_rx: &Receiver<(f32, f32, f32, f32)>,
+    duration_tx: &Sender<Duration>,
+) -> Ev3Result<()> {
     let left_sensor = ColorSensor::get(SensorPort::In2)?;
     let right_sensor = ColorSensor::get(SensorPort::In3)?;
     left_sensor.set_mode_rgb_raw()?;
     right_sensor.set_mode_rgb_raw()?;
 
-    let buttons = Ev3Button::new()?;
+    left_motor.set_polarity(LargeMotor::POLARITY_INVERSED)?;
-    println!("waiting for button press");
+    right_motor.set_polarity(LargeMotor::POLARITY_INVERSED)?;
-    while !buttons.is_enter() {
+    left_motor.set_stop_action(LargeMotor::STOP_ACTION_BRAKE)?;
-        draw_ui(1.0, 0.3, 0.1, 50.0)?;
+    right_motor.set_stop_action(LargeMotor::STOP_ACTION_BRAKE)?;
-        buttons.process();
+
-    }
+    'outer: while !stop.load(Ordering::Relaxed) {
-    println!("button pressed");
+        let (kp, ki, kd, speed) = pid_rx.recv().unwrap_or_else(|_| {
+            stop.store(true, Ordering::Relaxed);
+            (0.0, 0.0, 0.0, 0.0)
+        });
+        let mut state = DriveState::Start;
+        let mut start = Instant::now();
+        let mut cycles = 0;
+        #[allow(clippy::cast_precision_loss)]
+        let max_speed = left_motor
+            .get_max_speed()?
+            .min(right_motor.get_max_speed()?) as f32;
+        //let mut pid = Pid::new(0.0, 1.0);
+        //pid.p(kp, 1.0).i(ki / 10.0, 1.0).d(kd / 100.0, 1.0);
+        //while state != DriveState::Finish {
+        left_motor.run_direct()?;
+        right_motor.run_direct()?;
+        loop {
+            if reset.swap(false, Ordering::Relaxed) {
+                left_motor.stop()?;
+                right_motor.stop()?;
+                break;
+            }
+            let left_rgb = left_sensor.get_rgb()?;
+            std::hint::black_box(left_rgb);
+            let right_rgb = right_sensor.get_rgb()?;
+            std::hint::black_box(right_rgb);
+            /*
+            if is_red(left_rgb) && is_red(right_rgb) {
+                match state {
+                    DriveState::Start => {
+                        start = Instant::now();
+                        cycles = 0;
+                        state = DriveState::RedLine;
+                    }
+                    DriveState::Measuring => {
+                        duration_tx
+                            .send(start.elapsed())
+                            .unwrap_or_else(|_| stop.store(true, Ordering::Relaxed));
+                        state = DriveState::Finish;
+                    }
+                    _ => (),
+                }
+            } else if !is_red(left_rgb) && !is_red(right_rgb) && state == DriveState::RedLine {
+                state = DriveState::Measuring;
+            }
+            let left_gray = gray(left_rgb);
+            let right_gray = gray(right_rgb);
+            let diff = right_gray - left_gray;
+            let measurement = diff / left_gray.max(right_gray).max(1.0);
+            let controll = pid.next_control_output(measurement).output;
+            let left_speed = (1.0 + controll).min(1.0);
+            let right_speed = (1.0 - controll).min(1.0);
+            #[allow(clippy::cast_possible_truncation)]
+            left_motor.set_speed_sp(/*(left_speed * speed * max_speed) as i32*/ 0)?;
+            #[allow(clippy::cast_possible_truncation)]
+            right_motor.set_speed_sp(/*(right_speed * speed * max_speed) as i32*/ 0)?;
+            left_motor.run_forever()?;
+            right_motor.run_forever()?;
+            */
+            left_motor.set_duty_cycle_sp(0)?;
+            right_motor.set_duty_cycle_sp(0)?;
+            cycles += 1;
+        }
+        println!(
+            "{} cycles in {}s ({}cycles/s)",
+            cycles,
+            start.elapsed().as_secs_f32(),
+            cycles * 1000 / start.elapsed().as_millis()
+        );
+        left_motor.stop()?;
+        right_motor.stop()?;
+    }
 
-    left_motor.set_polarity("inversed")?;
-    right_motor.set_polarity("inversed")?;
     Ok(())
 }
 
-fn draw_ui(kp: f32, ki: f32, kd: f32, speed: f32) -> Ev3Result<()> {
+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
+}
+
+fn gray((r, g, b): (i32, i32, i32)) -> f32 {
+    #[allow(clippy::cast_precision_loss)]
+    {
+        (r + g + b) as f32 / 3.0
+    }
+}
+
+macro_rules! button_impl {
+    ($($name: ident),*) => {
+        paste! {
+
+            struct SmartEv3Buttons {
+                buttons: Ev3Button,
+                $(
+                    [<last_ $name>]: bool,
+                )*
+            }
+
+            impl SmartEv3Buttons {
+                fn new() -> Ev3Result<Self> {
+                    let buttons = Ev3Button::new()?;
+                    buttons.process();
+                    $(
+                        let [<last_ $name>] = buttons.[<is_ $name>]();
+                    )*
+                        Ok(Self {
+                            buttons,
+                            $(
+                                [<last_ $name>],
+                            )*
+                        })
+                }
+
+                fn process(&mut self) {
+                    $(
+                        self.[<last_ $name>] = self.buttons.[<is_ $name>]();
+                    )*
+                    self.buttons.process();
+                }
+
+                $(
+                    #[allow(dead_code)]
+                    fn [<is_ $name>](&self) -> bool {
+                        self.buttons.[<is_ $name>]()
+                    }
+
+                    #[allow(dead_code)]
+                    fn [<is_ $name _pressed>](&self) -> bool {
+                        self.buttons.[<is_ $name>]() && !self.[<last_ $name>]
+                    }
+
+                    #[allow(dead_code)]
+                    fn [<is_ $name _released>](&self) -> bool {
+                        !self.buttons.[<is_ $name>]() && self.[<last_ $name>]
+                    }
+                )*
+            }
+        }
+    };
+}
+
+button_impl!(up, down, left, right, enter, backspace);
+
+fn ui_thread(
+    stop: &Arc<AtomicBool>,
+    reset: &Arc<AtomicBool>,
+    pid_tx: &Sender<(f32, f32, f32, f32)>,
+    duration_tx: &Receiver<Duration>,
+) -> Ev3Result<()> {
+    use Parameter::{Kd, Ki, Kp, Speed};
+    use State::{Driving, Finish, Settings};
+    const STEP: f32 = 0.01;
+    let mut buttons = SmartEv3Buttons::new()?;
+    let mut display = Ev3Display::new()?;
+    let mut kp = 1.0;
+    let mut ki = 0.3;
+    let mut kd = 0.1;
+    let mut speed = 0.1;
+    let mut selected = Kp;
+    let mut state = Settings;
+    let mut state_changed = true;
+    let mut duration = Duration::default();
+    // exit on backspace
+    while !(buttons.is_backspace() || stop.load(Ordering::Relaxed)) {
+        buttons.process();
+        match state {
+            Settings => {
+                let new_selected = match (buttons.is_left_pressed(), buttons.is_right_pressed()) {
+                    (true, false) => match selected {
+                        Kp => Speed,
+                        Ki => Kp,
+                        Kd => Ki,
+                        Speed => Kd,
+                    },
+                    (false, true) => match selected {
+                        Kp => Ki,
+                        Ki => Kd,
+                        Kd => Speed,
+                        Speed => Kp,
+                    },
+                    _ => selected,
+                };
+                let changed = match (buttons.is_up(), buttons.is_down()) {
+                    (true, false) => {
+                        match selected {
+                            Kp => kp += STEP,
+                            Ki => ki += STEP,
+                            Kd => kd += STEP,
+                            Speed => speed += STEP,
+                        }
+                        true
+                    }
+                    (false, true) => {
+                        match selected {
+                            Kp => kp -= STEP,
+                            Ki => ki -= STEP,
+                            Kd => kd -= STEP,
+                            Speed => speed -= STEP,
+                        }
+                        true
+                    }
+                    _ => false,
+                } || new_selected != selected;
+                kp = kp.clamp(0.0, 10.0);
+                ki = ki.clamp(0.0, 10.0);
+                kd = kd.clamp(0.0, 10.0);
+                speed = speed.clamp(0.0, 1.0);
+                selected = new_selected;
+                if buttons.is_enter_pressed() {
+                    state = Driving;
+                    state_changed = true;
+                    pid_tx
+                        .send((kp, ki, kd, speed))
+                        .unwrap_or_else(|_| stop.store(true, Ordering::Relaxed));
+                    continue;
+                }
+                if changed || state_changed {
+                    display.draw_settings(kp, ki, kd, speed, selected);
+                    sleep(Duration::from_secs_f32(1.0 / 5.0));
+                } else {
+                    sleep(Duration::from_secs_f32(1.0 / 30.0));
+                }
+            }
+            Driving => {
+                if buttons.is_enter_pressed() {
+                    reset.store(true, Ordering::Relaxed);
+                    state = Settings;
+                    state_changed = true;
+                    continue;
+                }
+                if let Ok(new_duration) = duration_tx.try_recv() {
+                    state = Finish;
+                    state_changed = true;
+                    duration = new_duration;
+                    continue;
+                }
+                if state_changed {
+                    display.draw_driving();
+                    state_changed = false;
+                }
+                sleep(Duration::from_secs_f32(1.0 / 30.0));
+            }
+            Finish => {
+                if buttons.is_enter_pressed() {
+                    state = Settings;
+                    state_changed = true;
+                    continue;
+                }
+                if state_changed {
+                    display.draw_finished(duration);
+                    state_changed = false;
+                }
+                sleep(Duration::from_secs_f32(1.0 / 30.0));
+            }
+        }
+        // limit display update rate
+    }
+    Ok(())
+}
+
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+enum Parameter {
+    Kp,
+    Ki,
+    Kd,
+    Speed,
+}
+
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+enum State {
+    Settings,
+    Driving,
+    Finish,
+}
+
+struct Ev3Display {
+    screen: Screen,
+    font: Font<'static>,
+    width: u8,
+    height: u8,
+    quater_x: u8,
+    half_x: u8,
+    quater_y: u8,
+    half_y: u8,
+    quater_scale: Scale,
+    half_scale: Scale,
+    last_state: Option<State>,
+}
+
+impl Ev3Display {
     const BACKGROUND: Rgb<u8> = Rgb([255; 3]);
     const FOREGROUND: Rgb<u8> = Rgb([0; 3]);
-    let mut screen = Screen::new()?;
+    const FONT_DATA: &[u8] = include_bytes!("../fonts/RobotoMono-Regular.ttf");
-    let center = screen.xres() / 2;
+
-    let quater = screen.yres() / 4;
+    fn new() -> Ev3Result<Self> {
-    screen.image.fill(BACKGROUND.0[0]);
+        let screen = Screen::new()?;
-    let input_field_width = center;
+        let font = Font::try_from_bytes(Self::FONT_DATA).unwrap();
-    let input_field_height = quater - 4;
+        let width = screen.xres().try_into().unwrap();
-    let input_field_x = (center - 2).try_into().unwrap();
+        let height = screen.yres().try_into().unwrap();
-    for i in 0..4 {
+        let half_x = width / 2;
-        draw_hollow_rect_mut(
+        let quater_x = width / 4;
-            &mut screen.image,
+        let half_y = height / 2;
-            Rect::at(input_field_x, (i * quater + 2).try_into().unwrap())
+        let quater_y = height / 4;
-                .of_size(input_field_width, input_field_height),
+        let quater_scale = Scale {
-            FOREGROUND,
+            x: f32::from(quater_y - 8) * 6.0 / 8.0,
+            y: f32::from(quater_y - 8),
+        };
+        let half_scale = Scale::uniform(f32::from(half_y - 8));
+
+        Ok(Self {
+            screen,
+            font,
+            width,
+            height,
+            quater_x,
+            half_x,
+            quater_y,
+            half_y,
+            quater_scale,
+            half_scale,
+            last_state: None,
+        })
+    }
+
+    fn draw_settings(&mut self, kp: f32, ki: f32, kd: f32, speed: f32, selected: Parameter) {
+        // clear the screen
+        if self.last_state == Some(State::Settings) {
+            draw_filled_rect_mut(
+                &mut self.screen.image,
+                Rect::at(0, self.half_y.into()).of_size(self.width.into(), self.half_y.into()),
+                Self::BACKGROUND,
+            );
+        } else {
+            self.screen.image.fill(Self::BACKGROUND.0[0]);
+            self.draw_centered_text(self.half_x.into(), 4, self.quater_scale, "Settings");
+            self.draw_centered_text(
+                i32::from(self.quater_x / 2),
+                i32::from(self.quater_y + 4),
+                self.quater_scale,
+                "kp",
+            );
+            self.draw_centered_text(
+                i32::from(self.quater_x / 2 + self.quater_x),
+                i32::from(self.quater_y + 4),
+                self.quater_scale,
+                "ki",
+            );
+            self.draw_centered_text(
+                i32::from(self.quater_x / 2 + self.half_x),
+                i32::from(self.quater_y + 4),
+                self.quater_scale,
+                "kd",
+            );
+            self.draw_centered_text(
+                i32::from(self.width - self.quater_x / 2),
+                i32::from(self.quater_y + 4),
+                self.quater_scale,
+                "spd",
             );
         }
-    draw_value(kp, &mut screen.image, center, quater, 0);
+        self.last_state = Some(State::Settings);
-    draw_value(ki, &mut screen.image, center, quater, 1);
-    draw_value(kd, &mut screen.image, center, quater, 2);
-    draw_value(speed, &mut screen.image, center, quater, 3);
-    screen.update();
-    Ok(())
-}
 
-fn draw_value(value: f32, canvas: &mut RgbImage, center: u32, quater: u32, index: u8) {
+        self.draw_setting(kp, selected == Parameter::Kp, 0);
-    const FOREGROUND: Rgb<u8> = Rgb([0; 3]);
+        self.draw_setting(ki, selected == Parameter::Ki, 1);
-    const FONT_DATA: &[u8] = include_bytes!("../fonts/RobotoMono-Regular.ttf");
+        self.draw_setting(kd, selected == Parameter::Kd, 2);
-    let font = Font::try_from_bytes(FONT_DATA).unwrap();
+        self.draw_setting(speed, selected == Parameter::Speed, 3);
-    draw_text_mut(
+
-        canvas,
+        self.screen.update();
-        FOREGROUND,
+    }
-        center.try_into().unwrap(),
+
-        i32::from(index) * i32::try_from(quater).unwrap() + 4,
+    fn draw_setting(&mut self, value: f32, selected: bool, index: u8) {
-        Scale::uniform((quater - 8) as f32),
+        let x = self.quater_x / 2 + self.quater_x * index;
-        &font,
+        self.draw_centered_text(
+            x.into(),
+            i32::from(self.quater_y * 2 + self.quater_y / 2) + 4,
+            self.quater_scale,
             format!("{value:2.2}").as_str(),
         );
+        let top_triangle = [
+            Point::new(x.into(), self.half_y.into()),
+            Point::new((x - 10).into(), (self.half_y + 10).into()),
+            Point::new((x + 10).into(), (self.half_y + 10).into()),
+        ];
+        let bottom_triangle = [
+            Point::new(x.into(), self.height.into()),
+            Point::new((x - 10).into(), (self.height - 10).into()),
+            Point::new((x + 10).into(), (self.height - 10).into()),
+        ];
+        if selected {
+            draw_polygon_mut(&mut self.screen.image, &top_triangle, Self::FOREGROUND);
+            draw_polygon_mut(&mut self.screen.image, &bottom_triangle, Self::FOREGROUND);
+        } else {
+            for (start, end) in [
+                (top_triangle[0], top_triangle[1]),
+                (top_triangle[1], top_triangle[2]),
+                (top_triangle[2], top_triangle[0]),
+                (bottom_triangle[0], bottom_triangle[1]),
+                (bottom_triangle[1], bottom_triangle[2]),
+                (bottom_triangle[2], bottom_triangle[0]),
+            ] {
+                draw_line_segment_mut(
+                    &mut self.screen.image,
+                    point2tuple(start),
+                    point2tuple(end),
+                    Self::FOREGROUND,
+                );
+            }
+        }
+    }
+
+    fn draw_driving(&mut self) {
+        // clear the screen
+        if self.last_state == Some(State::Driving) {
+            draw_filled_rect_mut(
+                &mut self.screen.image,
+                Rect::at(0, self.half_y.into()).of_size(self.width.into(), self.half_y.into()),
+                Self::BACKGROUND,
+            );
+        } else {
+            self.screen.image.fill(Self::BACKGROUND.0[0]);
+            self.draw_centered_text(self.half_x.into(), 4, self.half_scale, "Fahrt");
+        }
+        self.last_state = Some(State::Driving);
+
+        self.screen.update();
+    }
+
+    fn draw_finished(&mut self, time: Duration) {
+        self.last_state = Some(State::Finish);
+        self.screen.image.fill(Self::BACKGROUND.0[0]);
+        self.draw_centered_text(self.half_x.into(), 4, self.half_scale, "Zeit:");
+        self.draw_centered_text(
+            self.half_x.into(),
+            i32::from(self.half_y) + 4,
+            self.half_scale,
+            format!("{:.2}s", time.as_secs_f32()).as_str(),
+        );
+        self.screen.update();
+    }
+
+    fn draw_centered_text(&mut self, x: i32, y: i32, scale: Scale, text: &str) {
+        let (width, _) = text_size(scale, &self.font, text);
+        draw_text_mut(
+            &mut self.screen.image,
+            Self::FOREGROUND,
+            x - width / 2,
+            y,
+            scale,
+            &self.font,
+            text,
+        );
+    }
 }
+
+const fn point2tuple(point: Point<i32>) -> (f32, f32) {
+    #[allow(clippy::cast_precision_loss)]
+    (point.x as f32, point.y as f32)
+}
+*/

upload.sh

@@ -0,0 +1,2 @@
+#!/bin/sh
+cargo build --release && scp target/armv5te-unknown-linux-musleabi/release/ev3dev-pid-linefollow robot@ev3dev:/home/robot/