ecki/Robotics-at-Home-with-Raspberry-Pi-Pico
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Current state of experiments

Browse Source
This commit is contained in:
Danny Staple
2022-09-05 11:08:03 +01:00
parent d7ba881234
commit df1d67e42d
15 changed files with 91 additions and 660 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch-10/4-wall-follow/robot.FCStd
BIN
View File
Binary file not shown.
ch-11/1-measuring_wheel_speeds/code.py
+6 -9
View File
@@ -1,8 +1,5 @@
import asyncio
import board
import busio
import robot
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
class Settings:
speed = 0.7
@@ -17,7 +14,7 @@ async def motor_speed_loop():
left_last = left_new
right_speed = robot.ticks_to_mm(right_new - right_last) / Settings.time_interval
right_last = right_new
uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode())
robot.uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode())
async def stop_motors_after(seconds):
await asyncio.sleep(seconds)
@@ -25,8 +22,8 @@ async def stop_motors_after(seconds):
async def command_handler():
while True:
if uart.in_waiting:
command = uart.readline().decode().strip()
if robot.uart.in_waiting:
command = robot.uart.readline().decode().strip()
if command.startswith("M"):
Settings.speed = float(command[1:])
elif command.startswith("T"):
@@ -35,12 +32,12 @@ async def command_handler():
robot.stop()
elif command.startswith("O"):
await asyncio.sleep(5)
asyncio.create_task(stop_motors_after(float(command[1:])))
robot.set_left(Settings.speed)
robot.set_right(Settings.speed)
asyncio.create_task(stop_motors_after(float(command[1:])))
elif command.startswith("?"):
uart.write(f"M{Settings.speed:.1f}\n".encode())
uart.write(f"T{Settings.time_interval:.1f}\n".encode())
robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
await asyncio.sleep(3)
await asyncio.sleep(0)
ch-11/1-measuring_wheel_speeds/robot.py
+3
View File
@@ -4,6 +4,9 @@ import pio_encoder
import busio
import adafruit_vl53l1x
import math
import busio
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
wheel_diameter_mm = 70
wheel_circumference_mm = math.pi * wheel_diameter_mm
ch-11/1.5-speed-control/code.py
-100
View File
@@ -1,100 +0,0 @@
import asyncio
import board
import busio
import robot
import time
import pid_controller
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
class Settings:
# work in ticks only - 1 rpm
speed = 0.5
time_interval = 0.2
motors_enabled = False
dead_zone = 0.2
class SpeedController:
def __init__(self, encoder, motor_fn) -> None:
self.encoder = encoder
self.motor_fn = motor_fn
#P1.4,I2.8,D1
#P0, I2.5, D1.1
self.pid = pid_controller.PIDController(0.1, 1.5, 0)
self.reset()
def reset(self):
self.last_ticks = self.encoder.read()
self.error = 0
self.control_signal = 0
self.pid.reset()
def update(self, dt):
current_ticks = self.encoder.read()
actual_speed_in_rpm = (current_ticks - self.last_ticks) / (dt * robot.ticks_per_revolution)
self.last_ticks = current_ticks
# calculate the error
self.error = (Settings.speed * Settings.motors_enabled) - actual_speed_in_rpm
# calculate the control signal
self.control_signal = self.pid.calculate(self.error, dt)
self.motor_fn(self.control_signal)
left = SpeedController(robot.left_encoder, robot.set_left)
async def speed_controller_loop():
last_time = time.monotonic()
while True:
await asyncio.sleep(Settings.time_interval)
current_time = time.monotonic()
dt = current_time - last_time
left.update(dt)
last_time = current_time
print(f"{dt:.2f}")
uart.write(f"{Settings.speed * Settings.motors_enabled}, {left.error:.2f},{left.control_signal:.2f}\n".encode())
async def stop_motors_after(seconds):
await asyncio.sleep(seconds)
Settings.motors_enabled = False
# robot.stop()
async def command_handler():
while True:
if uart.in_waiting:
command = uart.readline().decode().strip()
# PID settings
if command.startswith("P"):
left.pid.kp = float(command[1:])
elif command.startswith("I"):
left.pid.ki = float(command[1:])
left.pid.reset()
elif command.startswith("D"):
left.pid.kd = float(command[1:])
# Speed settings
elif command.startswith("M"):
Settings.speed = float(command[1:])
elif command.startswith("T"):
Settings.time_interval = float(command[1:])
# Start/stop commands
elif command == "O":
Settings.motors_enabled = False
elif command.startswith("O"):
asyncio.create_task(stop_motors_after(float(command[1:])))
Settings.motors_enabled = True
left.reset()
# Print settings
elif command.startswith("?"):
uart.write(f"M{Settings.speed:.1f}\n".encode())
uart.write(f"T{Settings.time_interval:.1f}\n".encode())
uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
await asyncio.sleep(3)
await asyncio.sleep(0)
try:
asyncio.create_task(speed_controller_loop())
asyncio.create_task(stop_motors_after(10))
Settings.motors_enabled = True
asyncio.run(command_handler())
finally:
robot.stop()
ch-11/1.5-speed-control/pid_controller.py
-27
View File
@@ -1,27 +0,0 @@
class PIDController:
def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
self.kp = kp
self.ki = ki
self.kd = kd
self.d_filter_gain = d_filter_gain
self.imax = imax
self.imin = imin
self.reset()
def reset(self):
self.integral = 0
self.error_prev = 0
self.derivative = 0
def calculate(self, error, dt):
self.integral += error * dt
if self.imax is not None and self.integral > self.imax:
self.integral = self.imax
if self.imin is not None and self.integral < self.imin:
self.integral = self.imin
# Add a low pass filter to the difference
difference = (error - self.error_prev) * self.d_filter_gain
self.error_prev += difference
self.derivative = difference / dt
return self.kp * error + self.ki * self.integral + self.kd * self.derivative
ch-11/1.5-speed-control/pio_encoder.py
-84
View File
@@ -1,84 +0,0 @@
import rp2pio
import adafruit_pioasm
import array
import asyncio
program = """
; use the osr for count
; input pins c1 c2
set y, 0 ; clear y
mov osr, y ; and clear osr
read:
; x will be the old value
; y the new values
mov x, y ; store old Y in x
in null, 32 ; Clear ISR - using y
in pins, 2 ; read two pins into y
mov y, isr
jmp x!=y, different ; Jump if its different
jmp read ; otherwise loop back to read
different:
; x has old value, y has new.
; extract the upper bit of X.
in x, 31 ; get bit 31 - old p1 (remember which direction it came in)
in null, 31 ; keep only 1 bit
mov x, isr ; put this back in x
jmp !x, c1_old_zero
c1_old_not_zero:
jmp pin, count_up
jmp count_down
c1_old_zero:
jmp pin, count_down
; fall through
count_up:
; for a clockwise move - we'll add 1 by inverting
mov x, ~ osr ; store inverted OSR on x
jmp x--, fake ; use jump to take off 1
fake:
mov x, ~ x ; invert back
jmp send
count_down:
; for a clockwise move, just take one off
mov x, osr ; store osr in x
jmp x--, send ; dec and send
send:
; send x.
mov isr, x ; send it
push noblock ; put ISR into input FIFO
mov osr, x ; put X back in OSR
jmp read ; loop back
"""
assembled = adafruit_pioasm.assemble(program)
class QuadratureEncoder:
def __init__(self, first_pin, second_pin, reversed=False):
"""Encoder with 2 pins. Must use sequential pins on the board"""
self.sm = rp2pio.StateMachine(
assembled,
frequency=0,
first_in_pin=first_pin,
jmp_pin=second_pin,
in_pin_count=2,
)
self.reversed = reversed
self._buffer = array.array("i", [0])
asyncio.create_task(self.poll_loop())
async def poll_loop(self):
while True:
await asyncio.sleep(0)
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
def read(self):
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]
ch-11/1.5-speed-control/robot.py
-68
View File
@@ -1,68 +0,0 @@
import board
import pwmio
import pio_encoder
import busio
import adafruit_vl53l1x
import math
wheel_diameter_mm = 70
wheel_circumference_mm = math.pi * wheel_diameter_mm
gear_ratio = 298
encoder_poles = 28
ticks_per_revolution = encoder_poles * gear_ratio
ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
def ticks_to_mm(ticks):
return ticks_to_mm_const * ticks
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
right_motor = motor_A1, motor_A2
left_motor = motor_B1, motor_B2
motor_dead_zone = 0.2
right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
def stop():
motor_A1.duty_cycle = 0
motor_A2.duty_cycle = 0
motor_B1.duty_cycle = 0
motor_B2.duty_cycle = 0
def set_speed(motor, speed):
# stop completely if in the dead zone
if abs(speed) < motor_dead_zone:
motor[0].duty_cycle = 0
motor[1].duty_cycle = 0
return
# Swap motor pins if we reverse the speed
if speed < 0:
direction = motor[1], motor[0]
speed = -speed
else:
direction = motor
speed = min(speed, 1) # limit to 1.0
max_speed = 2 ** 16 - 1
direction[0].duty_cycle = int(max_speed * speed)
direction[1].duty_cycle = 0
def set_left(speed):
set_speed(left_motor, speed)
def set_right(speed):
set_speed(right_motor, speed)
ch-11/2-speed-control-acceleration/code.py
-103
View File
@@ -1,103 +0,0 @@
import asyncio
import time
import robot
import pid_controller
class Settings:
speed = 0.17
distance = 1
time_interval = 0.2
motors_enabled = False
class SpeedController:
def __init__(self, encoder, motor_fn):
self.encoder = encoder
self.motor_fn = motor_fn
self.pid = pid_controller.PIDController(3, 0, 1)
self.reset()
def reset(self):
self.last_ticks = self.encoder.read()
self.error = 0
self.speed = 0
self.actual_speed = 0
self.pid.reset()
def update(self, dt):
current_ticks = self.encoder.read()
speed_in_ticks = (current_ticks - self.last_ticks) / dt
self.last_ticks = current_ticks
self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
# calculate the error
self.error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
# calculate the control signal
control_signal = self.pid.calculate(self.error, dt)
self.speed += control_signal
self.motor_fn(self.speed)
left = SpeedController(robot.left_encoder, robot.set_left)
right = SpeedController(robot.right_encoder, robot.set_right)
async def motor_speed_loop():
last_time = time.monotonic()
while True:
await asyncio.sleep(Settings.time_interval)
current_time = time.monotonic()
dt = current_time - last_time
last_time = current_time
left.update(dt)
right.update(dt)
robot.uart.write(f"0, {left.actual_speed:.2f},{Settings.speed:.2f}\n".encode())
async def stop_motors_after(seconds):
await asyncio.sleep(seconds)
Settings.motors_enabled = False
async def command_handler():
while True:
if robot.uart.in_waiting:
command = robot.uart.readline().decode().strip()
# PID settings
if command.startswith("P"):
left.pid.kp = float(command[1:])
right.pid.kp = float(command[1:])
elif command.startswith("I"):
left.pid.ki = float(command[1:])
left.pid.reset()
right.pid.ki = float(command[1:])
right.pid.reset()
elif command.startswith("D"):
left.pid.kd = float(command[1:])
right.pid.kd = float(command[1:])
elif command.startswith("T"):
Settings.time_interval = float(command[1:])
# Speed settings
elif command.startswith("M"):
Settings.speed = float(command[1:])
# Start/stop commands
elif command == "O":
Settings.motors_enabled = False
elif command.startswith("O"):
await asyncio.sleep(5)
asyncio.create_task(stop_motors_after(float(command[1:])))
Settings.motors_enabled = True
left.reset()
right.reset()
# Print settings
elif command.startswith("?"):
robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
robot.uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
await asyncio.sleep(3)
await asyncio.sleep(0)
try:
asyncio.create_task(motor_speed_loop())
asyncio.run(command_handler())
finally:
robot.stop()
ch-11/2-speed-control-acceleration/pid_controller.py
-27
View File
@@ -1,27 +0,0 @@
class PIDController:
def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
self.kp = kp
self.ki = ki
self.kd = kd
self.d_filter_gain = d_filter_gain
self.imax = imax
self.imin = imin
self.reset()
def reset(self):
self.integral = 0
self.error_prev = 0
self.derivative = 0
def calculate(self, error, dt):
self.integral += error * dt
if self.imax is not None and self.integral > self.imax:
self.integral = self.imax
if self.imin is not None and self.integral < self.imin:
self.integral = self.imin
# Add a low pass filter to the difference
difference = (error - self.error_prev) * self.d_filter_gain
self.error_prev += difference
self.derivative = difference / dt
return self.kp * error + self.ki * self.integral + self.kd * self.derivative
ch-11/2-speed-control-acceleration/pio_encoder.py
-84
View File
@@ -1,84 +0,0 @@
import rp2pio
import adafruit_pioasm
import array
import asyncio
program = """
; use the osr for count
; input pins c1 c2
set y, 0 ; clear y
mov osr, y ; and clear osr
read:
; x will be the old value
; y the new values
mov x, y ; store old Y in x
in null, 32 ; Clear ISR - using y
in pins, 2 ; read two pins into y
mov y, isr
jmp x!=y, different ; Jump if its different
jmp read ; otherwise loop back to read
different:
; x has old value, y has new.
; extract the upper bit of X.
in x, 31 ; get bit 31 - old p1 (remember which direction it came in)
in null, 31 ; keep only 1 bit
mov x, isr ; put this back in x
jmp !x, c1_old_zero
c1_old_not_zero:
jmp pin, count_up
jmp count_down
c1_old_zero:
jmp pin, count_down
; fall through
count_up:
; for a clockwise move - we'll add 1 by inverting
mov x, ~ osr ; store inverted OSR on x
jmp x--, fake ; use jump to take off 1
fake:
mov x, ~ x ; invert back
jmp send
count_down:
; for a clockwise move, just take one off
mov x, osr ; store osr in x
jmp x--, send ; dec and send
send:
; send x.
mov isr, x ; send it
push noblock ; put ISR into input FIFO
mov osr, x ; put X back in OSR
jmp read ; loop back
"""
assembled = adafruit_pioasm.assemble(program)
class QuadratureEncoder:
def __init__(self, first_pin, second_pin, reversed=False):
"""Encoder with 2 pins. Must use sequential pins on the board"""
self.sm = rp2pio.StateMachine(
assembled,
frequency=0,
first_in_pin=first_pin,
jmp_pin=second_pin,
in_pin_count=2,
)
self.reversed = reversed
self._buffer = array.array("i", [0])
asyncio.create_task(self.poll_loop())
async def poll_loop(self):
while True:
await asyncio.sleep(0)
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
def read(self):
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]
ch-11/2-speed-control-acceleration/robot.py
-65
View File
@@ -1,65 +0,0 @@
import board
import pwmio
import pio_encoder
import busio
import adafruit_vl53l1x
import math
import busio
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
wheel_diameter_mm = 70
wheel_circumference_mm = math.pi * wheel_diameter_mm
gear_ratio = 298
encoder_poles = 28
ticks_per_revolution = encoder_poles * gear_ratio
ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
def ticks_to_mm(ticks):
return ticks_to_mm_const * ticks
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
right_motor = motor_A1, motor_A2
left_motor = motor_B1, motor_B2
right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
def stop():
motor_A1.duty_cycle = 0
motor_A2.duty_cycle = 0
motor_B1.duty_cycle = 0
motor_B2.duty_cycle = 0
def set_speed(motor, speed):
# Swap motor pins if we reverse the speed
if speed < 0:
direction = motor[1], motor[0]
speed = -speed
else:
direction = motor
speed = min(speed, 1) # limit to 1.0
max_speed = 2 ** 16 - 1
direction[0].duty_cycle = int(max_speed * speed)
direction[1].duty_cycle = 0
def set_left(speed):
set_speed(left_motor, speed)
def set_right(speed):
set_speed(right_motor, speed)
ch-11/2-speed-control/code.py
+13 -27
View File
@@ -13,26 +13,27 @@ class SpeedController:
def __init__(self, encoder, motor_fn):
self.encoder = encoder
self.motor_fn = motor_fn
self.pid = pid_controller.PIDController(0, 6, 0)
self.pid = pid_controller.PIDController(3, 0, 1)
self.reset()
def reset(self):
self.last_ticks = self.encoder.read()
self.error = 0
self.control_signal = 0
self.pwm = 0
self.actual_speed = 0
self.pid.reset()
def update(self, dt):
current_ticks = self.encoder.read()
speed_in_ticks = (current_ticks - self.last_ticks) / dt
self.last_ticks = current_ticks
speed_in_m_per_s = robot.ticks_to_mm(speed_in_ticks) / 1000
# calculate the error
self.error = (Settings.speed * Settings.motors_enabled) - speed_in_m_per_s
self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
# calculate the error
error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
# calculate the control signal
self.control_signal = self.pid.calculate(self.error, dt)
self.motor_fn(self.control_signal)
control_signal = self.pid.calculate(error, dt)
self.pwm += control_signal
self.motor_fn(self.pwm)
left = SpeedController(robot.left_encoder, robot.set_left)
right = SpeedController(robot.right_encoder, robot.set_right)
@@ -47,7 +48,7 @@ async def motor_speed_loop():
last_time = current_time
left.update(dt)
right.update(dt)
robot.uart.write(f"0, {left.error:.2f},{right.error:.2f}\n".encode())
robot.uart.write(f"0, {left.actual_speed:.2f},{Settings.speed * Settings.motors_enabled:.2f}\n".encode())
async def stop_motors_after(seconds):
@@ -59,24 +60,10 @@ async def command_handler():
while True:
if robot.uart.in_waiting:
command = robot.uart.readline().decode().strip()
# PID settings
if command.startswith("P"):
left.pid.kp = float(command[1:])
right.pid.kp = float(command[1:])
elif command.startswith("I"):
left.pid.ki = float(command[1:])
left.pid.reset()
right.pid.ki = float(command[1:])
right.pid.reset()
elif command.startswith("D"):
left.pid.kd = float(command[1:])
right.pid.kd = float(command[1:])
if command.startswith("M"):
Settings.speed = float(command[1:])
elif command.startswith("T"):
Settings.time_interval = float(command[1:])
# Speed settings
elif command.startswith("M"):
Settings.speed = float(command[1:])
# Start/stop commands
elif command == "O":
Settings.motors_enabled = False
elif command.startswith("O"):
@@ -88,7 +75,6 @@ async def command_handler():
# Print settings
elif command.startswith("?"):
robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
robot.uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
await asyncio.sleep(3)
await asyncio.sleep(0)
ch-11/2-speed-control/robot.py
+4
View File
@@ -45,6 +45,10 @@ def stop():
def set_speed(motor, speed):
# Swap motor pins if we reverse the speed
if abs(speed) < 0.1:
motor[0].duty_cycle = 0
motor[1].duty_cycle = 1
return
if speed < 0:
direction = motor[1], motor[0]
speed = -speed
ch-11/3-known-distance-acceleration/code.py
+57 -66
View File
@@ -3,58 +3,65 @@ import time
import robot
import pid_controller
class Settings:
speed = 0.17
time_interval = 0.2
motors_enabled = False
class SpeedController:
class DistanceController:
def __init__(self, encoder, motor_fn):
self.encoder = encoder
self.motor_fn = motor_fn
self.pid = pid_controller.PIDController(3, 0, 1)
# accel
# self.pid = pid_controller.PIDController(0.00000, 0, 0.00008, d_filter_gain=1)
self.pid = pid_controller.PIDController(0.00000, 0.0000, 0.00001, d_filter_gain=1)
self.start_ticks = self.encoder.read()
self.pwm = 0
self.error = 0
def update(self, dt, expected, debug=False):
actual = self.encoder.read() - self.start_ticks
# calculate the error
self.error = expected - actual
# calculate the control signal
control_signal = self.pid.calculate(self.error, dt)
print(control_signal)
# self.pwm += control_signal
if debug:
robot.uart.write(f"0, {expected:.2f},{actual:.2f}\n".encode())
# self.motor_fn(self.pwm)
self.motor_fn(control_signal)
class DistanceTracker:
def __init__(self):
self.speed = 0.10
self.time_interval = 0.2
self.reset()
def reset(self):
self.last_ticks = self.encoder.read()
self.error = 0
self.speed = 0
self.actual_speed = 0
self.pid.reset()
self.start_time = time.monotonic()
self.total_distance_in_ticks = 0
self.total_time = 0.1
def update(self, dt):
current_ticks = self.encoder.read()
speed_in_ticks = (current_ticks - self.last_ticks) / dt
self.last_ticks = current_ticks
self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
# calculate the error
self.error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
# calculate the control signal
control_signal = self.pid.calculate(self.error, dt)
self.speed += control_signal
self.motor_fn(self.speed)
def set_distance(self, new_distance):
self.reset()
self.total_distance_in_ticks = robot.mm_to_ticks(new_distance * 1000)
self.total_time = new_distance / self.speed
async def loop(self):
left = DistanceController(robot.left_encoder, robot.set_left)
right = DistanceController(robot.right_encoder, robot.set_right)
last_time = time.monotonic()
while True:
await asyncio.sleep(self.time_interval)
current_time = time.monotonic()
dt = current_time - last_time
last_time = current_time
elapsed_time = current_time - self.start_time
time_proportion = min(1, elapsed_time / self.total_time)
expected = time_proportion * self.total_distance_in_ticks
left.update(dt, expected, debug=True)
right.update(dt, expected)
left = SpeedController(robot.left_encoder, robot.set_left)
right = SpeedController(robot.right_encoder, robot.set_right)
async def motor_speed_loop():
last_time = time.monotonic()
while True:
await asyncio.sleep(Settings.time_interval)
current_time = time.monotonic()
dt = current_time - last_time
last_time = current_time
left.update(dt)
right.update(dt)
robot.uart.write(f"0, {left.actual_speed:.2f},{Settings.speed:.2f}\n".encode())
async def stop_motors_after(seconds):
await asyncio.sleep(seconds)
Settings.motors_enabled = False
distance_tracker = DistanceTracker()
async def command_handler():
@@ -62,41 +69,25 @@ async def command_handler():
if robot.uart.in_waiting:
command = robot.uart.readline().decode().strip()
# PID settings
if command.startswith("P"):
left.pid.kp = float(command[1:])
right.pid.kp = float(command[1:])
elif command.startswith("I"):
left.pid.ki = float(command[1:])
left.pid.reset()
right.pid.ki = float(command[1:])
right.pid.reset()
elif command.startswith("D"):
left.pid.kd = float(command[1:])
right.pid.kd = float(command[1:])
if command.startswith("M"):
distance_tracker.speed = float(command[1:])
elif command.startswith("T"):
Settings.time_interval = float(command[1:])
# Speed settings
elif command.startswith("M"):
Settings.speed = float(command[1:])
distance_tracker.time_interval = float(command[1:])
# Start/stop commands
elif command == "O":
Settings.motors_enabled = False
distance_tracker.set_distance(0)
elif command.startswith("O"):
await asyncio.sleep(5)
asyncio.create_task(stop_motors_after(float(command[1:])))
Settings.motors_enabled = True
left.reset()
right.reset()
distance_tracker.set_distance(float(command[1:]))
# Print settings
elif command.startswith("?"):
robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
robot.uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
robot.uart.write(f"M{distance_tracker.speed:.1f}\n".encode())
robot.uart.write(f"T{distance_tracker.time_interval:.1f}\n".encode())
await asyncio.sleep(3)
await asyncio.sleep(0)
try:
asyncio.create_task(motor_speed_loop())
asyncio.create_task(distance_tracker.loop())
asyncio.run(command_handler())
finally:
robot.stop()
ch-11/3-known-distance-acceleration/robot.py
+8
View File
@@ -18,6 +18,10 @@ ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
def ticks_to_mm(ticks):
return ticks_to_mm_const * ticks
def mm_to_ticks(mm):
return mm / ticks_to_mm_const
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
@@ -45,6 +49,10 @@ def stop():
def set_speed(motor, speed):
# Swap motor pins if we reverse the speed
if abs(speed) < 0.1:
motor[0].duty_cycle = 0
motor[1].duty_cycle = 1
return
if speed < 0:
direction = motor[1], motor[0]
speed = -speed