max/ev3dev-pid-linefollow
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

single threaded approach

Browse Source
This commit is contained in:
Max Känner 2023-01-28 23:39:10 +01:00
parent 4c2474ef6e
commit df8781d8d2
6 changed files with 878 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Cargo.lock generated
View File

@ -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"

3
Cargo.toml
View File

@ -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"

59
src/buttons.rs Normal file
View File

@ -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);

197
src/display.rs Normal file
View File

@ -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)
}

653
src/main.rs
View File

@ -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()?;
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,
$(
[<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()?;
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<Self> {
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<u8> = 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<i32>) -> (f32, f32) {
#[allow(clippy::cast_precision_loss)]
(point.x as f32, point.y as f32)
}
*/

2
upload.sh Executable file
View File

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