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

ch-10 clean

Browse Source
This commit is contained in:
Danny Staple 2022-03-20 22:31:18 +00:00
parent c632222255
commit b595f3be54
14 changed files with 369 additions and 356 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
ch-10/1-follow/follow.py
View File

@ -2,53 +2,54 @@ import time
import robot import robot
import pid import pid
class FollowObject: class FollowObject:
def __init__(self): def __init__(self):
self.max_speed = 0.9 self.max_speed = 0.9
self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15) self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15)
self.last_time = time.monotonic_ns() self.last_time = time.monotonic()
self.left_dist = 0 self.left_dist = 0
self.pid_output = 0 self.pid_output = 0
def setup_robot(self): def setup_robot(self):
robot.left_distance.distance_mode = 1 robot.left_distance.distance_mode = 1
def movement_update(self): def movement_update(self):
# do we have data # do we have data
if robot.left_distance.data_ready: if robot.left_distance.data_ready:
self.left_dist = robot.left_distance.distance self.left_dist = robot.left_distance.distance
# calculate time delta # calculate time delta
new_time = time.monotonic_ns() new_time = time.monotonic()
time_delta = new_time - self.last_time time_delta = new_time - self.last_time
self.last_time = new_time self.last_time = new_time
# get speeds from pid # get speeds from pid
self.pid_output = self.follow_pid.update(self.left_dist, time_delta) self.pid_output = self.follow_pid.update(self.left_dist, time_delta)
speed = min(self.max_speed, self.pid_output) speed = min(self.max_speed, self.pid_output)
speed = max(-self.max_speed, speed) speed = max(-self.max_speed, speed)
# make movements # make movements
robot.set_left(speed) robot.set_left(speed)
robot.set_right(speed) robot.set_right(speed)
# reset and loop # reset and loop
robot.left_distance.clear_interrupt() robot.left_distance.clear_interrupt()
def main_loop(self): def main_loop(self):
robot.left_distance.start_ranging() robot.left_distance.start_ranging()
self.last_time = time.monotonic() while True:
while True: self.movement_update()
self.movement_update()
def start(self):
print("Starting")
try:
self.setup_robot()
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
def start(self):
print("Starting")
try:
self.setup_robot()
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
FollowObject().start() FollowObject().start()

34
ch-10/1-follow/pid.py
View File

@ -1,23 +1,23 @@
class PID: class PID:
def __init__(self, proportional_k, integral_k, differential_k, set_point): def __init__(self, proportional_k, integral_k, differential_k, set_point):
self.proportional_k = proportional_k self.proportional_k = proportional_k
self.integral_k = integral_k self.integral_k = integral_k
self.differential_k = differential_k self.differential_k = differential_k
self.set_point = set_point self.set_point = set_point
self.error_sum = 0 self.error_sum = 0
self.last_value = 0 self.last_value = 0
def update(self, measurement, time_delta): def update(self, measurement, time_delta):
error_value = measurement - self.set_point error_value = measurement - self.set_point
proportional = error_value * self.proportional_k proportional = error_value * self.proportional_k
# calculate integral # calculate integral
self.error_sum += error_value * time_delta self.error_sum += error_value * time_delta
integral = self.error_sum * self.integral_k integral = self.error_sum * self.integral_k
self.last_value = error_value self.last_value = error_value
differentiated_error = (error_value - self.last_value) / time_delta differentiated_error = (error_value - self.last_value) / time_delta
differential = differentiated_error * self.differential_k differential = differentiated_error * self.differential_k
return proportional + integral + differential return proportional + integral + differential

38
ch-10/1-follow/pio_encoder.py
View File

@ -54,24 +54,24 @@ send:
assembled = adafruit_pioasm.assemble(program) assembled = adafruit_pioasm.assemble(program)
class QuadratureEncoder: class QuadratureEncoder:
def __init__(self, first_pin, second_pin, reversed=False): def __init__(self, first_pin, second_pin, reversed=False):
"""Encoder with 2 pins. Must use sequential pins on the board""" """Encoder with 2 pins. Must use sequential pins on the board"""
self.sm = rp2pio.StateMachine( self.sm = rp2pio.StateMachine(
assembled, assembled,
frequency=0, frequency=0,
first_in_pin=first_pin, first_in_pin=first_pin,
jmp_pin=second_pin, jmp_pin=second_pin,
in_pin_count=2 in_pin_count=2,
) )
self.reversed = reversed self.reversed = reversed
self._buffer = array.array('i', [0]) self._buffer = array.array("i", [0])
def read(self):
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]
def read(self):
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]

8
ch-10/1-follow/robot.py
View File

@ -29,6 +29,7 @@ def stop():
motor_B1.duty_cycle = 0 motor_B1.duty_cycle = 0
motor_B2.duty_cycle = 0 motor_B2.duty_cycle = 0
def set_speed(motor, speed): def set_speed(motor, speed):
# Swap motor pins if we reverse the speed # Swap motor pins if we reverse the speed
if speed < 0: if speed < 0:
@ -36,15 +37,16 @@ def set_speed(motor, speed):
speed = -speed speed = -speed
else: else:
direction = motor direction = motor
speed = min(speed, 1) # limit to 1.0 speed = min(speed, 1) # limit to 1.0
max_speed = 2**16-1 max_speed = 2 ** 16 - 1
direction[0].duty_cycle = int(max_speed * speed) direction[0].duty_cycle = int(max_speed * speed)
direction[1].duty_cycle = 0 direction[1].duty_cycle = 0
def set_left(speed): def set_left(speed):
set_speed(left_motor, speed) set_speed(left_motor, speed)
def set_right(speed): def set_right(speed):
set_speed(right_motor, speed) set_speed(right_motor, speed)

145
ch-10/2-graphing/graphing_follow.py
View File

@ -10,88 +10,91 @@ import pid
class FollowObject: class FollowObject:
def __init__(self): def __init__(self):
self.max_speed = 0.9 self.max_speed = 0.9
self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15) self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15)
self.wifi = None self.wifi = None
self.server = None self.server = None
self.last_time = time.monotonic_ns() self.last_time = time.monotonic()
self.left_dist = 0 self.left_dist = 0
self.pid_output = 0 self.pid_output = 0
def setup_robot(self): def setup_robot(self):
robot.left_distance.distance_mode = 1 robot.left_distance.distance_mode = 1
def setup_wifi(self, app): def setup_wifi(self, app):
print("Setting up wifi.") print("Setting up wifi.")
self.wifi, esp = robot_wifi.connect_to_wifi() self.wifi, esp = robot_wifi.connect_to_wifi()
self.server = adafruit_esp32spi_wsgiserver.WSGIServer( self.server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
80, adafruit_esp32spi_wsgiserver.set_interface(esp)
application=app print("Starting server")
)
adafruit_esp32spi_wsgiserver.set_interface(esp)
print("Starting server")
self.server.start() self.server.start()
ip_int = ".".join(str(int(n)) for n in esp.ip_address) ip_int = ".".join(str(int(n)) for n in esp.ip_address)
print(f"IP Address is {ip_int}") print(f"IP Address is {ip_int}")
def index(self, request): def index(self, request):
return 200, [('Content-Type', 'application/json')], [json.dumps( return (
{ 200,
"last_value": self.follow_pid.last_value, [("Content-Type", "application/json")],
"pid_output": self.pid_output, [
"time": self.last_time json.dumps(
} {
)] "last_value": self.follow_pid.last_value,
"pid_output": self.pid_output,
"time": self.last_time,
}
)
],
)
def movement_update(self): def movement_update(self):
# do we have data # do we have data
if robot.left_distance.data_ready: if robot.left_distance.data_ready:
self.left_dist = robot.left_distance.distance self.left_dist = robot.left_distance.distance
# calculate time delta # calculate time delta
new_time = time.monotonic_ns() new_time = time.monotonic()
time_delta = new_time - self.last_time time_delta = new_time - self.last_time
self.last_time = new_time self.last_time = new_time
# get speeds from pid # get speeds from pid
self.pid_output = self.follow_pid.update(self.left_dist, time_delta) self.pid_output = self.follow_pid.update(self.left_dist, time_delta)
speed = self.pid_output * self.max_speed speed = self.pid_output * self.max_speed
# make movements # make movements
robot.set_left(speed) robot.set_left(speed)
robot.set_right(speed) robot.set_right(speed)
# reset and loop # reset and loop
robot.left_distance.clear_interrupt() robot.left_distance.clear_interrupt()
def main_loop(self): def main_loop(self):
robot.left_distance.start_ranging() robot.left_distance.start_ranging()
self.last_time = time.monotonic() while True:
while True: try:
try: self.movement_update()
self.movement_update() self.server.update_poll()
self.server.update_poll() except RuntimeError as e:
except RuntimeError as e: print(f"Server poll error: {type(e)}, {e}")
print(f"Server poll error: {type(e)}, {e}") robot.stop()
robot.stop() print(f"Resetting ESP...")
print(f"Resetting ESP...") self.wifi.reset()
self.wifi.reset() print("Reset complete.")
print("Reset complete.")
def start(self):
app = WSGIApp()
app.route("/")(self.index)
print("Starting")
try:
self.setup_robot()
self.setup_wifi(app)
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
def start(self):
app = WSGIApp()
app.route("/")(self.index)
print("Starting")
try:
self.setup_robot()
self.setup_wifi(app)
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
FollowObject().start() FollowObject().start()

60
ch-10/2-graphing/pid.py
View File

@ -2,41 +2,41 @@ from black import err
class PID: class PID:
def __init__(self, proportional_k, integral_k, differential_k, set_point): def __init__(self, proportional_k, integral_k, differential_k, set_point):
self.proportional_k = proportional_k self.proportional_k = proportional_k
self.integral_k = integral_k self.integral_k = integral_k
self.differential_k = differential_k self.differential_k = differential_k
self.set_point = set_point self.set_point = set_point
self.error_sum = 0 self.error_sum = 0
self.last_value = 0 self.last_value = 0
self.min_output = -1 self.min_output = -1
self.max_output = 1 self.max_output = 1
self.dead_zone = 0.3 self.dead_zone = 0.3
def update(self, measurement, time_delta): def update(self, measurement, time_delta):
error_value = measurement - self.set_point error_value = measurement - self.set_point
proportional = error_value * self.proportional_k proportional = error_value * self.proportional_k
# calculate integral # calculate integral
self.error_sum += error_value * time_delta self.error_sum += error_value * time_delta
# clamp it # clamp it
self.error_sum = min(self.max_output, self.error_sum) self.error_sum = min(self.max_output, self.error_sum)
self.error_sum = max(self.min_output, self.error_sum) self.error_sum = max(self.min_output, self.error_sum)
integral = self.error_sum * self.integral_k integral = self.error_sum * self.integral_k
differentiated_error = (error_value - self.last_value) / time_delta differentiated_error = (error_value - self.last_value) / time_delta
differential = differentiated_error * self.differential_k differential = differentiated_error * self.differential_k
self.last_value = error_value self.last_value = error_value
output = proportional + integral + differential output = proportional + integral + differential
# clamp output # clamp output
if abs(output) < self.dead_zone: if abs(output) < self.dead_zone:
output = 0 output = 0
else: else:
output = min(self.max_output, output) output = min(self.max_output, output)
output = max(self.min_output, output) output = max(self.min_output, output)
return output return output

38
ch-10/2-graphing/pio_encoder.py
View File

@ -54,24 +54,24 @@ send:
assembled = adafruit_pioasm.assemble(program) assembled = adafruit_pioasm.assemble(program)
class QuadratureEncoder: class QuadratureEncoder:
def __init__(self, first_pin, second_pin, reversed=False): def __init__(self, first_pin, second_pin, reversed=False):
"""Encoder with 2 pins. Must use sequential pins on the board""" """Encoder with 2 pins. Must use sequential pins on the board"""
self.sm = rp2pio.StateMachine( self.sm = rp2pio.StateMachine(
assembled, assembled,
frequency=0, frequency=0,
first_in_pin=first_pin, first_in_pin=first_pin,
jmp_pin=second_pin, jmp_pin=second_pin,
in_pin_count=2 in_pin_count=2,
) )
self.reversed = reversed self.reversed = reversed
self._buffer = array.array('i', [0]) self._buffer = array.array("i", [0])
def read(self):
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]
def read(self):
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]

8
ch-10/2-graphing/robot.py
View File

@ -29,6 +29,7 @@ def stop():
motor_B1.duty_cycle = 0 motor_B1.duty_cycle = 0
motor_B2.duty_cycle = 0 motor_B2.duty_cycle = 0
def set_speed(motor, speed): def set_speed(motor, speed):
# Swap motor pins if we reverse the speed # Swap motor pins if we reverse the speed
if speed < 0: if speed < 0:
@ -36,15 +37,16 @@ def set_speed(motor, speed):
speed = -speed speed = -speed
else: else:
direction = motor direction = motor
speed = min(speed, 1) # limit to 1.0 speed = min(speed, 1) # limit to 1.0
max_speed = 2**16-1 max_speed = 2 ** 16 - 1
direction[0].duty_cycle = int(max_speed * speed) direction[0].duty_cycle = int(max_speed * speed)
direction[1].duty_cycle = 0 direction[1].duty_cycle = 0
def set_left(speed): def set_left(speed):
set_speed(left_motor, speed) set_speed(left_motor, speed)
def set_right(speed): def set_right(speed):
set_speed(right_motor, speed) set_speed(right_motor, speed)

24
ch-10/2-graphing/robot_wifi.py
View File

@ -5,21 +5,21 @@ from adafruit_esp32spi import adafruit_esp32spi
from adafruit_esp32spi import adafruit_esp32spi_wifimanager from adafruit_esp32spi import adafruit_esp32spi_wifimanager
try: try:
from secrets import secrets from secrets import secrets
except ImportError: except ImportError:
print("WiFi secrets are kept in secrets.py, please add them there!") print("WiFi secrets are kept in secrets.py, please add them there!")
raise raise
def connect_to_wifi(): def connect_to_wifi():
esp32_cs = DigitalInOut(board.GP10) esp32_cs = DigitalInOut(board.GP10)
esp32_ready = DigitalInOut(board.GP9) esp32_ready = DigitalInOut(board.GP9)
esp32_reset = DigitalInOut(board.GP8) esp32_reset = DigitalInOut(board.GP8)
spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12) spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset) esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
esp.reset() esp.reset()
wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets) wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
wifi.connect() wifi.connect()
return wifi, esp return wifi, esp

60
ch-10/3-wall-follow/pid.py
View File

@ -1,39 +1,39 @@
class PID: class PID:
def __init__(self, proportional_k, integral_k, differential_k, set_point): def __init__(self, proportional_k, integral_k, differential_k, set_point):
self.proportional_k = proportional_k self.proportional_k = proportional_k
self.integral_k = integral_k self.integral_k = integral_k
self.differential_k = differential_k self.differential_k = differential_k
self.set_point = set_point self.set_point = set_point
self.error_sum = 0 self.error_sum = 0
self.last_value = 0 self.last_value = 0
self.min_output = -1 self.min_output = -1
self.max_output = 1 self.max_output = 1
self.dead_zone = 0.3 self.dead_zone = 0.3
def update(self, measurement, time_delta): def update(self, measurement, time_delta):
error_value = measurement - self.set_point error_value = measurement - self.set_point
proportional = error_value * self.proportional_k proportional = error_value * self.proportional_k
# calculate integral # calculate integral
self.error_sum += error_value * time_delta self.error_sum += error_value * time_delta
# clamp it # clamp it
self.error_sum = min(self.max_output, self.error_sum) self.error_sum = min(self.max_output, self.error_sum)
self.error_sum = max(self.min_output, self.error_sum) self.error_sum = max(self.min_output, self.error_sum)
integral = self.error_sum * self.integral_k integral = self.error_sum * self.integral_k
differentiated_error = (error_value - self.last_value) / time_delta differentiated_error = (error_value - self.last_value) / time_delta
differential = differentiated_error * self.differential_k differential = differentiated_error * self.differential_k
self.last_value = error_value self.last_value = error_value
output = proportional + integral + differential output = proportional + integral + differential
# clamp output # clamp output
if abs(output) < self.dead_zone: if abs(output) < self.dead_zone:
output = 0 output = 0
else: else:
output = min(self.max_output, output) output = min(self.max_output, output)
output = max(self.min_output, output) output = max(self.min_output, output)
return output return output

38
ch-10/3-wall-follow/pio_encoder.py
View File

@ -54,24 +54,24 @@ send:
assembled = adafruit_pioasm.assemble(program) assembled = adafruit_pioasm.assemble(program)
class QuadratureEncoder: class QuadratureEncoder:
def __init__(self, first_pin, second_pin, reversed=False): def __init__(self, first_pin, second_pin, reversed=False):
"""Encoder with 2 pins. Must use sequential pins on the board""" """Encoder with 2 pins. Must use sequential pins on the board"""
self.sm = rp2pio.StateMachine( self.sm = rp2pio.StateMachine(
assembled, assembled,
frequency=0, frequency=0,
first_in_pin=first_pin, first_in_pin=first_pin,
jmp_pin=second_pin, jmp_pin=second_pin,
in_pin_count=2 in_pin_count=2,
) )
self.reversed = reversed self.reversed = reversed
self._buffer = array.array('i', [0]) self._buffer = array.array("i", [0])
def read(self):
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]
def read(self):
while self.sm.in_waiting:
self.sm.readinto(self._buffer)
if self.reversed:
return -self._buffer[0]
else:
return self._buffer[0]

8
ch-10/3-wall-follow/robot.py
View File

@ -29,6 +29,7 @@ def stop():
motor_B1.duty_cycle = 0 motor_B1.duty_cycle = 0
motor_B2.duty_cycle = 0 motor_B2.duty_cycle = 0
def set_speed(motor, speed): def set_speed(motor, speed):
# Swap motor pins if we reverse the speed # Swap motor pins if we reverse the speed
if speed < 0: if speed < 0:
@ -36,15 +37,16 @@ def set_speed(motor, speed):
speed = -speed speed = -speed
else: else:
direction = motor direction = motor
speed = min(speed, 1) # limit to 1.0 speed = min(speed, 1) # limit to 1.0
max_speed = 2**16-1 max_speed = 2 ** 16 - 1
direction[0].duty_cycle = int(max_speed * speed) direction[0].duty_cycle = int(max_speed * speed)
direction[1].duty_cycle = 0 direction[1].duty_cycle = 0
def set_left(speed): def set_left(speed):
set_speed(left_motor, speed) set_speed(left_motor, speed)
def set_right(speed): def set_right(speed):
set_speed(right_motor, speed) set_speed(right_motor, speed)

24
ch-10/3-wall-follow/robot_wifi.py
View File

@ -5,21 +5,21 @@ from adafruit_esp32spi import adafruit_esp32spi
from adafruit_esp32spi import adafruit_esp32spi_wifimanager from adafruit_esp32spi import adafruit_esp32spi_wifimanager
try: try:
from secrets import secrets from secrets import secrets
except ImportError: except ImportError:
print("WiFi secrets are kept in secrets.py, please add them there!") print("WiFi secrets are kept in secrets.py, please add them there!")
raise raise
def connect_to_wifi(): def connect_to_wifi():
esp32_cs = DigitalInOut(board.GP10) esp32_cs = DigitalInOut(board.GP10)
esp32_ready = DigitalInOut(board.GP9) esp32_ready = DigitalInOut(board.GP9)
esp32_reset = DigitalInOut(board.GP8) esp32_reset = DigitalInOut(board.GP8)
spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12) spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset) esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
esp.reset() esp.reset()
wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets) wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
wifi.connect() wifi.connect()
return wifi, esp return wifi, esp

149
ch-10/3-wall-follow/wall_follow.py
View File

@ -11,91 +11,94 @@ import robot_wifi
class FollowWallApp: class FollowWallApp:
def __init__(self) -> None: def __init__(self) -> None:
self.speed = 0.6 self.speed = 0.6
self.max_deflection = 0.4 self.max_deflection = 0.4
self.follow_pid = pid.PID(0.1, 0.5, 0, 15) self.follow_pid = pid.PID(0.1, 0.5, 0, 15)
self.follow_pid.dead_zone = 0.6 self.follow_pid.dead_zone = 0.6
self.wifi = None self.wifi = None
self.server = None self.server = None
self.last_time = time.monotonic_ns() self.last_time = time.monotonic()
self.left_dist = 0 self.left_dist = 0
self.pid_output = 0 self.pid_output = 0
def setup_robot(self): def setup_robot(self):
robot.left_distance.distance_mode = 1 robot.left_distance.distance_mode = 1
def setup_wifi(self, app): def setup_wifi(self, app):
print("Setting up wifi.") print("Setting up wifi.")
self.wifi, esp = robot_wifi.connect_to_wifi() self.wifi, esp = robot_wifi.connect_to_wifi()
self.server = adafruit_esp32spi_wsgiserver.WSGIServer( self.server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
80, adafruit_esp32spi_wsgiserver.set_interface(esp)
application=app print("Starting server")
)
adafruit_esp32spi_wsgiserver.set_interface(esp)
print("Starting server")
self.server.start() self.server.start()
ip_int = ".".join(str(int(n)) for n in esp.ip_address) ip_int = ".".join(str(int(n)) for n in esp.ip_address)
print(f"IP Address is {ip_int}") print(f"IP Address is {ip_int}")
def index(self, request): def index(self, request):
return 200, [('Content-Type', 'application/json')], [json.dumps( return (
{ 200,
"last_value": self.follow_pid.last_value, [("Content-Type", "application/json")],
"pid_output": self.pid_output, [
"time": self.last_time json.dumps(
} {
)] "last_value": self.follow_pid.last_value,
"pid_output": self.pid_output,
"time": self.last_time,
}
)
],
)
def movement_update(self): def movement_update(self):
# do we have data # do we have data
if robot.left_distance.data_ready: if robot.left_distance.data_ready:
self.left_dist = robot.left_distance.distance self.left_dist = robot.left_distance.distance
# calculate time delta # calculate time delta
new_time = time.monotonic_ns() new_time = time.monotonic()
time_delta = new_time - self.last_time time_delta = new_time - self.last_time
self.last_time = new_time self.last_time = new_time
# get turn from pid # get turn from pid
self.pid_output = self.follow_pid.update(self.left_dist, time_delta) self.pid_output = self.follow_pid.update(self.left_dist, time_delta)
deflection = self.pid_output * self.max_deflection deflection = self.pid_output * self.max_deflection
# make movements # make movements
robot.set_left(self.speed - deflection) robot.set_left(self.speed - deflection)
robot.set_right(self.speed + deflection) robot.set_right(self.speed + deflection)
# reset and loop # reset and loop
robot.left_distance.clear_interrupt() robot.left_distance.clear_interrupt()
def main_loop(self): def main_loop(self):
robot.left_distance.start_ranging() robot.left_distance.start_ranging()
self.last_time = time.monotonic() while True:
while True: try:
try: self.movement_update()
self.movement_update() self.server.update_poll()
self.server.update_poll() except RuntimeError as e:
except RuntimeError as e: print(f"Server poll error: {type(e)}, {e}")
print(f"Server poll error: {type(e)}, {e}") robot.stop()
robot.stop() print(f"Resetting ESP...")
print(f"Resetting ESP...") self.wifi.reset()
self.wifi.reset() print("Reset complete.")
print("Reset complete.")
def start(self):
app = WSGIApp()
app.route("/")(self.index)
print("Starting")
try:
self.setup_robot()
self.setup_wifi(app)
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
def start(self):
app = WSGIApp()
app.route("/")(self.index)
print("Starting")
try:
self.setup_robot()
self.setup_wifi(app)
self.main_loop()
finally:
robot.stop()
robot.left_distance.clear_interrupt()
robot.left_distance.stop_ranging()
FollowWallApp().start() FollowWallApp().start()