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

Remove old code

Browse Source
This commit is contained in:
Max Känner 2023-01-29 15:16:01 +01:00
parent a5dc6f15af
commit a4ac3b9c9c
1 changed files with 1 additions and 550 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

551
src/main.rs
View File

@ -46,7 +46,7 @@ fn main() -> Result<(), Box<dyn Error>> {
&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<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()?;
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,
$(
[<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]);
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,
);
}
}
const fn point2tuple(point: Point<i32>) -> (f32, f32) {
#[allow(clippy::cast_precision_loss)]
(point.x as f32, point.y as f32)
}
*/