diff --git a/Cargo.lock b/Cargo.lock index 014f0d5..1662347 100644 --- a/Cargo.lock +++ b/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" diff --git a/Cargo.toml b/Cargo.toml index c2e8b69..cf03784 100644 --- a/Cargo.toml +++ b/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" diff --git a/src/buttons.rs b/src/buttons.rs new file mode 100644 index 0000000..390493f --- /dev/null +++ b/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, + $( + []: bool, + )* + } + + impl SmartEv3Buttons { + pub fn new() -> Ev3Result { + let buttons = Ev3Button::new()?; + buttons.process(); + $( + let [] = buttons.[](); + )* + Ok(Self { + buttons, + $( + [], + )* + }) + } + + pub fn process(&mut self) { + $( + self.[] = self.buttons.[](); + )* + self.buttons.process(); + } + + $( + #[allow(dead_code)] + pub fn [](&self) -> bool { + self.buttons.[]() + } + + #[allow(dead_code)] + pub fn [](&self) -> bool { + self.buttons.[]() && !self.[] + } + + #[allow(dead_code)] + pub fn [](&self) -> bool { + !self.buttons.[]() && self.[] + } + )* + } + } + }; +} + +button_impl!(up, down, left, right, enter, backspace); diff --git a/src/display.rs b/src/display.rs new file mode 100644 index 0000000..cf040c7 --- /dev/null +++ b/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 = Rgb([255; 3]); +const FOREGROUND: Rgb = 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) -> (f32, f32) { + #[allow(clippy::cast_precision_loss)] + (point.x as f32, point.y as f32) +} diff --git a/src/main.rs b/src/main.rs index 2945add..0c59fe8 100644 --- a/src/main.rs +++ b/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> { + 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> { + 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, + reset: &Arc, + pid_rx: &Receiver<(f32, f32, f32, f32)>, + duration_tx: &Sender, +) -> 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()?; - println!("waiting for button press"); - while !buttons.is_enter() { - draw_ui(1.0, 0.3, 0.1, 50.0)?; - buttons.process(); - } - println!("button pressed"); + 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)?; + + 'outer: while !stop.load(Ordering::Relaxed) { + 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, + $( + []: bool, + )* + } + + impl SmartEv3Buttons { + fn new() -> Ev3Result { + let buttons = Ev3Button::new()?; + buttons.process(); + $( + let [] = buttons.[](); + )* + Ok(Self { + buttons, + $( + [], + )* + }) + } + + fn process(&mut self) { + $( + self.[] = self.buttons.[](); + )* + self.buttons.process(); + } + + $( + #[allow(dead_code)] + fn [](&self) -> bool { + self.buttons.[]() + } + + #[allow(dead_code)] + fn [](&self) -> bool { + self.buttons.[]() && !self.[] + } + + #[allow(dead_code)] + fn [](&self) -> bool { + !self.buttons.[]() && self.[] + } + )* + } + } + }; +} + +button_impl!(up, down, left, right, enter, backspace); + +fn ui_thread( + stop: &Arc, + reset: &Arc, + pid_tx: &Sender<(f32, f32, f32, f32)>, + duration_tx: &Receiver, +) -> 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, +} + +impl Ev3Display { const BACKGROUND: Rgb = Rgb([255; 3]); const FOREGROUND: Rgb = Rgb([0; 3]); - let mut screen = Screen::new()?; - let center = screen.xres() / 2; - let quater = screen.yres() / 4; - screen.image.fill(BACKGROUND.0[0]); - let input_field_width = center; - let input_field_height = quater - 4; - let input_field_x = (center - 2).try_into().unwrap(); - for i in 0..4 { - draw_hollow_rect_mut( - &mut screen.image, - Rect::at(input_field_x, (i * quater + 2).try_into().unwrap()) - .of_size(input_field_width, input_field_height), - FOREGROUND, + const FONT_DATA: &[u8] = include_bytes!("../fonts/RobotoMono-Regular.ttf"); + + fn new() -> Ev3Result { + let screen = Screen::new()?; + let font = Font::try_from_bytes(Self::FONT_DATA).unwrap(); + let width = screen.xres().try_into().unwrap(); + let height = screen.yres().try_into().unwrap(); + let half_x = width / 2; + let quater_x = width / 4; + let half_y = height / 2; + let quater_y = height / 4; + let quater_scale = Scale { + 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", + ); + } + self.last_state = Some(State::Settings); + + self.draw_setting(kp, selected == Parameter::Kp, 0); + self.draw_setting(ki, selected == Parameter::Ki, 1); + self.draw_setting(kd, selected == Parameter::Kd, 2); + self.draw_setting(speed, selected == Parameter::Speed, 3); + + self.screen.update(); + } + + fn draw_setting(&mut self, value: f32, selected: bool, index: u8) { + let x = self.quater_x / 2 + self.quater_x * index; + 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, ); } - draw_value(kp, &mut screen.image, center, quater, 0); - 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) { - const FOREGROUND: Rgb = Rgb([0; 3]); - const FONT_DATA: &[u8] = include_bytes!("../fonts/RobotoMono-Regular.ttf"); - let font = Font::try_from_bytes(FONT_DATA).unwrap(); - draw_text_mut( - canvas, - FOREGROUND, - center.try_into().unwrap(), - i32::from(index) * i32::try_from(quater).unwrap() + 4, - Scale::uniform((quater - 8) as f32), - &font, - format!("{value:2.2}").as_str(), - ); +const fn point2tuple(point: Point) -> (f32, f32) { + #[allow(clippy::cast_precision_loss)] + (point.x as f32, point.y as f32) } +*/ diff --git a/upload.sh b/upload.sh new file mode 100755 index 0000000..b48c0fa --- /dev/null +++ b/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/