From a4ac3b9c9cad5de97d6abf8a238824249395639c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Max=20K=C3=A4nner?= Date: Sun, 29 Jan 2023 15:16:01 +0100 Subject: [PATCH] Remove old code --- src/main.rs | 551 +--------------------------------------------------- 1 file changed, 1 insertion(+), 550 deletions(-) diff --git a/src/main.rs b/src/main.rs index b9397e7..32b9a5d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -46,7 +46,7 @@ fn main() -> Result<(), Box> { &sensor, &mut buttons, setpoint, - (k_p / 10.0, k_i / 100.0, k_d / 100.0), + (k_p / 10.0, k_i / 10000.0, k_d / 10000.0), v, ); left_motor.stop().and(right_motor.stop())?; @@ -255,552 +255,3 @@ const fn is_red((r, g, b): (i32, i32, i32)) -> bool { // check for not black and green and blue significantly lower than red r > 32 && g < threshold && b < threshold } - -/* -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_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 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 -} - -#[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()?; - - 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()?; - } - - Ok(()) -} - -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]); - 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, - ); - } -} - -const fn point2tuple(point: Point) -> (f32, f32) { - #[allow(clippy::cast_precision_loss)] - (point.x as f32, point.y as f32) -} -*/