From ac3b3b1e18cd2a51b4c851d2c417284a206d5204 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jakub=20Van=C4=9Bk?= <linuxtardis@gmail.com>
Date: Wed, 24 May 2023 13:16:04 +0200
Subject: [PATCH] Update linefollower for presentation
---
programs/examples/ev3/linefollower.py | 153 +++++++++++++++-----------
1 file changed, 91 insertions(+), 62 deletions(-)
diff --git a/programs/examples/ev3/linefollower.py b/programs/examples/ev3/linefollower.py
index 540d333..9ecbbd4 100644
--- a/programs/examples/ev3/linefollower.py
+++ b/programs/examples/ev3/linefollower.py
@@ -1,6 +1,7 @@
import sys
import utime
+from micropython import const
from lib.cube.sh1106 import SH1106_I2C
from lib.ev3 import ColorSensor, Motor
@@ -11,29 +12,88 @@ CALIBRATION_RPM = 30
CALIBRATION_TIME = 1000
CALIBRATION_WAIT_UNTIL_DONE = 100
REG_SP = 0.5
-REG_KP = 75
-REG_KD = 5000
-REG_BIAS = 40
-REG_OUT_SIGN = +1
-OPTIMAL_DT = 10
+REG_KP = 120
+REG_KD = 20000
+REG_KD_DNOW = 0.50
+REG_BIAS = 60
+APPROX_DT = 2
+
+MODE_KP = const(0)
+MODE_KD = const(1)
+MODE_SPD = const(2)
+MODE_DNOW = const(3)
+NUM_MODES = const(4)
+
+
+class TunerUI:
+ def __init__(self, cube: Robot):
+ self.Kp = REG_KP
+ self.Kd = REG_KD
+ self.Dnow = REG_KD_DNOW
+ self.bias = REG_BIAS
+ self.cube = cube
+ self.mode = MODE_KP
+ self.last_buttons = cube.buttons.pressed()
+
+ def render_ui(self):
+ display = self.cube.display
+ display.fill(0)
+ display.centered_text("Open-Cube", 0)
+ display.centered_text("Linefollower", 8)
+
+ display.centered_text("Live tuning", 24)
+ display.text(f"{'*' if self.mode == MODE_KP else ' '} Kp = {self.Kp:.1f}", 0, 32)
+ display.text(f"{'*' if self.mode == MODE_KD else ' '} Kd = {self.Kd:.1f}", 0, 40)
+ display.text(f"{'*' if self.mode == MODE_SPD else ' '} Pwr = {self.bias:.1f}", 0, 48)
+ display.text(f"{'*' if self.mode == MODE_DNOW else ' '} Dnow = {self.Dnow:.2f}", 0, 56)
+ display.show()
+
+ def update_ui(self, force_render: bool = False):
+ now = self.cube.buttons.pressed()
+ last = self.last_buttons
+ self.last_buttons = now
+
+ if now[Button.UP] and not last[Button.UP]:
+ self.mode = (self.mode - 1) % NUM_MODES
+ force_render = True
+ if now[Button.DOWN] and not last[Button.DOWN]:
+ self.mode = (self.mode + 1) % NUM_MODES
+ force_render = True
+ if now[Button.LEFT] and not last[Button.LEFT]:
+ if self.mode == MODE_KP:
+ self.Kp -= 5
+ elif self.mode == MODE_KD:
+ self.Kd -= 500
+ elif self.mode == MODE_SPD:
+ self.bias -= 5
+ elif self.mode == MODE_DNOW:
+ self.Dnow -= 0.1
+ force_render = True
+ if now[Button.RIGHT] and not last[Button.RIGHT]:
+ if self.mode == MODE_KP:
+ self.Kp += 5
+ elif self.mode == MODE_KD:
+ self.Kd += 500
+ elif self.mode == MODE_SPD:
+ self.bias += 5
+ elif self.mode == MODE_DNOW:
+ self.Dnow += 0.1
+ force_render = True
+
+ if force_render:
+ self.render_ui()
class LineFollower:
def __init__(self, robot: Robot):
self.robot = robot
self.display = robot.display
- self.cs = ColorSensor(Sensor.S1)
+ self.cs = ColorSensor(Port.S1)
self.lm = Motor(Port.M1)
- self.rm = Motor(Port.M4)
+ self.rm = Motor(Port.M2)
self.light_min = None
self.light_max = None
- self.light_sp = None
- self.loop_times = [0] * 1000
- self.loop_times_idx = 0
- self.loop_count = 0
- self.loop_min = None
- self.loop_max = None
- self.loop_skip = True
+ self.ui = TunerUI(robot)
def wait_for_sensors(self):
self.display.fill(0)
@@ -48,63 +108,32 @@ class LineFollower:
self.wait_for_sensors()
self.calibrate_light()
- last_t = utime.ticks_ms()
- utime.sleep_ms(1)
err_last = None
+ derr_last = None
- self.display.fill(0)
- self.display.text("Running...", 0, 0, 1)
- self.display.show()
- while True:
- now_t = utime.ticks_ms()
- delta_t = utime.ticks_diff(now_t, last_t)
- last_t = now_t
+ self.ui.render_ui()
+ while True:
light = self.read_calibrated_light()
err = REG_SP - light
- if err_last is not None:
- derr = (err - err_last) / delta_t
- else:
+ if err_last is None:
derr = 0
+ else:
+ derr_now = (err - err_last) / APPROX_DT
+ if derr_last is None:
+ derr = derr_now
+ else:
+ derr = self.ui.Dnow * derr_now + (1 - self.ui.Dnow) * derr_last
+ derr_last = derr
err_last = err
- u = REG_KP * err + REG_KD * derr
-
- spd_r = REG_BIAS + u * REG_OUT_SIGN
- spd_l = REG_BIAS - u * REG_OUT_SIGN
- self.rm.dc(round(-spd_r))
- self.lm.dc(round(-spd_l))
-
- self.poll_buttons()
-
- elapsed = utime.ticks_diff(utime.ticks_ms(), last_t)
- if elapsed < OPTIMAL_DT:
- utime.sleep_ms(OPTIMAL_DT - elapsed - 1)
-
- def poll_buttons(self):
- global REG_BIAS
- self.robot.buttons.read_value()
- buttons = self.robot.buttons.pressed()
-
- pressed = None
- if buttons[Button.UP]:
- REG_BIAS += 5
- pressed = Button.UP
- elif buttons[Button.DOWN]:
- REG_BIAS -= 5
- pressed = Button.DOWN
- if pressed is not None:
- self.display.fill(0)
- self.display.text("> Custom speed <", 0, 0, 1)
- self.display.text(f"Kp={REG_KP:3d}", 0, 10, 1)
- self.display.text(f"Kd={REG_KD:3d}", 0, 18, 1)
- self.display.text(f"Spd={REG_BIAS:3d}", 0, 26, 1)
- self.display.show()
- while True:
- self.robot.buttons.read_value()
- if self.robot.buttons.pressed()[pressed]:
- utime.sleep_ms(1)
+ u = self.ui.Kp * err + self.ui.Kd * derr
+
+ self.lm.dc(round(self.ui.bias + u))
+ self.rm.dc(round(self.ui.bias - u))
+
+ self.ui.update_ui()
def calibrate_light(self):
self.display.fill(0)
--
GitLab