Extract line following into its own function and Add calibration

This commit is contained in:
Max Känner 2023-01-29 00:30:49 +01:00
parent df8781d8d2
commit ce2919580d
4 changed files with 152 additions and 38 deletions

21
Cargo.lock generated
View File

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

View File

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

View File

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

View File

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