From 4f7f3542bf1bd1c3ea0ed3a065af573b091d40a9 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Thu, 17 Mar 2022 23:06:16 +0000 Subject: [PATCH] Chapter 10 follow behaviors --- .deploy/ch-10-1-follow.sh | 5 ++ .deploy/ch-10-2-graphing.sh | 5 ++ ch-10/1-follow/follow.py | 45 ++++++++++ ch-10/1-follow/pid.py | 19 ++++ ch-10/1-follow/pio_encoder.py | 77 ++++++++++++++++ ch-10/1-follow/robot.py | 50 +++++++++++ ch-10/2-graphing/graphing_follow.py | 101 +++++++++++++++++++++ ch-10/2-graphing/pc/live_graph.py | 51 +++++++++++ ch-10/2-graphing/pc/requirements.txt | 2 + ch-10/2-graphing/pid.py | 34 +++++++ ch-10/2-graphing/pio_encoder.py | 77 ++++++++++++++++ ch-10/2-graphing/robot.py | 50 +++++++++++ ch-10/2-graphing/robot_wifi.py | 25 ++++++ ch-10/3-wall-follow/pc/live_graph.py | 52 +++++++++++ ch-10/3-wall-follow/pc/requirements.txt | 0 ch-10/3-wall-follow/pid.py | 19 ++++ ch-10/3-wall-follow/pio_encoder.py | 77 ++++++++++++++++ ch-10/3-wall-follow/robot.py | 50 +++++++++++ ch-10/3-wall-follow/robot_wifi.py | 25 ++++++ ch-10/3-wall-follow/wall_follow.py | 115 ++++++++++++++++++++++++ 20 files changed, 879 insertions(+) create mode 100755 .deploy/ch-10-1-follow.sh create mode 100755 .deploy/ch-10-2-graphing.sh create mode 100644 ch-10/1-follow/follow.py create mode 100644 ch-10/1-follow/pid.py create mode 100644 ch-10/1-follow/pio_encoder.py create mode 100755 ch-10/1-follow/robot.py create mode 100644 ch-10/2-graphing/graphing_follow.py create mode 100644 ch-10/2-graphing/pc/live_graph.py create mode 100644 ch-10/2-graphing/pc/requirements.txt create mode 100644 ch-10/2-graphing/pid.py create mode 100644 ch-10/2-graphing/pio_encoder.py create mode 100755 ch-10/2-graphing/robot.py create mode 100644 ch-10/2-graphing/robot_wifi.py create mode 100644 ch-10/3-wall-follow/pc/live_graph.py create mode 100644 ch-10/3-wall-follow/pc/requirements.txt create mode 100644 ch-10/3-wall-follow/pid.py create mode 100644 ch-10/3-wall-follow/pio_encoder.py create mode 100755 ch-10/3-wall-follow/robot.py create mode 100644 ch-10/3-wall-follow/robot_wifi.py create mode 100644 ch-10/3-wall-follow/wall_follow.py diff --git a/.deploy/ch-10-1-follow.sh b/.deploy/ch-10-1-follow.sh new file mode 100755 index 0000000..fcc5117 --- /dev/null +++ b/.deploy/ch-10-1-follow.sh @@ -0,0 +1,5 @@ +.deploy/send-it.sh \ + ch-10/1-follow/follow.py \ + ch-10/1-follow/robot.py \ + ch-10/1-follow/pio_encoder.py \ + ch-10/1-follow/pid.py diff --git a/.deploy/ch-10-2-graphing.sh b/.deploy/ch-10-2-graphing.sh new file mode 100755 index 0000000..5ed2385 --- /dev/null +++ b/.deploy/ch-10-2-graphing.sh @@ -0,0 +1,5 @@ +.deploy/send-it.sh \ + ch-10/2-graphing/graphing_follow.py \ + ch-10/2-graphing/robot.py \ + ch-10/2-graphing/pio_encoder.py \ + ch-10/2-graphing/pid.py diff --git a/ch-10/1-follow/follow.py b/ch-10/1-follow/follow.py new file mode 100644 index 0000000..553b943 --- /dev/null +++ b/ch-10/1-follow/follow.py @@ -0,0 +1,45 @@ +import robot +import time +import pid + +robot.left_distance.distance_mode = 1 + +max_speed = 0.9 +set_point = 15 + +robot.left_distance.start_ranging() + +follow_pid = pid.PID(0.1, 0, 0) +last_time = time.monotonic() +print("Starting") +try: + while True: + # do we have data + if robot.left_distance.data_ready: + left_dist = robot.left_distance.distance + + # get error value + error_value = left_dist - set_point + + # calculate time delta + new_time = time.monotonic() + time_delta = new_time - last_time + last_time = new_time + + # get speeds from pid + speed = min(max_speed, follow_pid.update(error_value, time_delta)) + speed = max(-max_speed, speed) + + # make movements + print(f"Dist: {left_dist}, Err: {error_value}, Speed: {speed}") + robot.set_left(speed) + robot.set_right(speed) + + # reset and loop + robot.left_distance.clear_interrupt() + time.sleep(0.1) + +finally: + robot.stop() + robot.left_distance.clear_interrupt() + robot.left_distance.stop_ranging() diff --git a/ch-10/1-follow/pid.py b/ch-10/1-follow/pid.py new file mode 100644 index 0000000..3d5c300 --- /dev/null +++ b/ch-10/1-follow/pid.py @@ -0,0 +1,19 @@ +class PID: + def __init__(self, proportional_k, integral_k, differential_k) -> None: + self.proportional_k = proportional_k + self.integral_k = integral_k + self.differential_k = differential_k + + self.integral = 0 + self.last_value = 0 + + def update(self, error_value, time_delta): + proportional = error_value * self.proportional_k + + self.integral += error_value * time_delta + integral = self.integral * self.integral_k + + differentiated_error = (error_value - self.last_value) / time_delta + differential = differentiated_error * self.differential_k + + return proportional + integral + differential diff --git a/ch-10/1-follow/pio_encoder.py b/ch-10/1-follow/pio_encoder.py new file mode 100644 index 0000000..e9202f8 --- /dev/null +++ b/ch-10/1-follow/pio_encoder.py @@ -0,0 +1,77 @@ +import rp2pio +import adafruit_pioasm +import array + +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]) + + 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] + \ No newline at end of file diff --git a/ch-10/1-follow/robot.py b/ch-10/1-follow/robot.py new file mode 100755 index 0000000..1b339ef --- /dev/null +++ b/ch-10/1-follow/robot.py @@ -0,0 +1,50 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x + + +motor_A1 = pwmio.PWMOut(board.GP17) +motor_A2 = pwmio.PWMOut(board.GP16) +motor_B1 = pwmio.PWMOut(board.GP18) +motor_B2 = pwmio.PWMOut(board.GP19) + +right_motor = motor_A1, motor_A2 +left_motor = motor_B1, motor_B2 + +right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True) +left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27) + +i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3) + +right_distance = adafruit_vl53l1x.VL53L1X(i2c0) +left_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) diff --git a/ch-10/2-graphing/graphing_follow.py b/ch-10/2-graphing/graphing_follow.py new file mode 100644 index 0000000..2dff4a7 --- /dev/null +++ b/ch-10/2-graphing/graphing_follow.py @@ -0,0 +1,101 @@ +import time +import json + +from adafruit_esp32spi import adafruit_esp32spi_wsgiserver +from adafruit_wsgi.wsgi_app import WSGIApp + +import pid +import robot +import robot_wifi + + +class FollowApp: + def __init__(self) -> None: + self.max_speed = 0.9 + self.follow_pid = pid.PID(0.1, 0.1, 0.015) + self.wifi = None + self.server = None + + self.last_time = time.monotonic_ns() + self.left_dist = 0 + self.pid_output = 0 + + def setup_robot(self): + robot.left_distance.distance_mode = 1 + + def setup_wifi(self, app): + print("Setting up wifi.") + self.wifi, esp = robot_wifi.connect_to_wifi() + self.server = adafruit_esp32spi_wsgiserver.WSGIServer( + 80, + application=app + ) + adafruit_esp32spi_wsgiserver.set_interface(esp) + print("Starting server") + + self.server.start() + ip_int = ".".join(str(int(n)) for n in esp.ip_address) + print(f"IP Address is {ip_int}") + + def index(self, request): + print("Handling request") + return 200, [('Content-Type', 'application/json')], [json.dumps( + { + "last_value": self.pid.last_value, + "pid_output": self.pid_output, + "time": self.last_time + } + )] + + def movement_update(self): + # do we have data + if robot.left_distance.data_ready: + self.left_dist = robot.left_distance.distance + + # calculate time delta + new_time = time.monotonic_ns() + time_delta = new_time - self.last_time + self.last_time = new_time + + # get speeds from pid + self.pid_output = self.follow_pid.update(self.left_dist, time_delta) + speed = min(self.max_speed, self.pid_output) + speed = max(-self.max_speed, speed) + + # make movements + if abs(speed) < 0.3: + speed = 0 + robot.set_left(speed) + robot.set_right(speed) + + # reset and loop + robot.left_distance.clear_interrupt() + + def main_loop(self): + robot.left_distance.start_ranging() + self.last_time = time.monotonic() + while True: + try: + self.movement_update() + self.server.update_poll() + except RuntimeError as e: + print(f"Server poll error: {type(e)}, {e}") + robot.stop() + print(f"Resetting ESP...") + self.wifi.reset() + 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() + +FollowApp().start() diff --git a/ch-10/2-graphing/pc/live_graph.py b/ch-10/2-graphing/pc/live_graph.py new file mode 100644 index 0000000..dcb7fe3 --- /dev/null +++ b/ch-10/2-graphing/pc/live_graph.py @@ -0,0 +1,51 @@ +""" Turn JSON data stream into graphs""" +import requests + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + +url = 'http://192.168.1.128' + + +class SensorStream: + def __init__(self) -> None: + self.reset() + + def reset(self): + self.error_values = [] + self.pid_outputs = [] + self.times = [] + + def sensor_stream(self, frame): + response = requests.get(url, timeout=1) + print(f"Content: {response.content}") + print(f"status: {response.status_code}") + + item = response.json() + + print(f"Received: {item}") + if self.times and item['time'] < self.times[-1]: + self.reset() + self.times.append(item['time']) + self.error_values.append(item['last_value']) + self.pid_outputs.append(item['pid_output']) + + if len(self.times) > 100: + self.times = self.times[-100:] + self.error_values = self.error_values[-100:] + self.pid_outputs = self.pid_outputs[-100:] + + plt.cla() # clear axes. + # plot the items + plt.plot(self.times, self.error_values, label="error") + plt.plot(self.times, self.pid_outputs, label="pid") + + plt.legend(loc='upper right') + + def start(self): + # Create the animation. GFC - get current figure. random_stream - callback func. + self.ani = FuncAnimation(plt.gcf(), self.sensor_stream, interval=200) + plt.tight_layout() + plt.show() + +SensorStream().start() diff --git a/ch-10/2-graphing/pc/requirements.txt b/ch-10/2-graphing/pc/requirements.txt new file mode 100644 index 0000000..66ed226 --- /dev/null +++ b/ch-10/2-graphing/pc/requirements.txt @@ -0,0 +1,2 @@ +matplotlib +requests diff --git a/ch-10/2-graphing/pid.py b/ch-10/2-graphing/pid.py new file mode 100644 index 0000000..cfe644e --- /dev/null +++ b/ch-10/2-graphing/pid.py @@ -0,0 +1,34 @@ +class PID: + def __init__(self, proportional_k, integral_k, differential_k, set_point): + self.proportional_k = proportional_k + self.integral_k = integral_k + self.differential_k = differential_k + self.set_point = set_point + + self.error_sum = 0 + self.last_value = 0 + self.min_output = -1 + self.max_output = 1 + + + def update(self, measurement, time_delta): + error_value = measurement - self.set_point + proportional = error_value * self.proportional_k + + # calculate integral + self.error_sum += error_value * time_delta + # clamp it + self.error_sum = min(self.max_output, self.error_sum) + self.error_sum = max(self.min_output, self.error_sum) + + integral = self.error_sum * self.integral_k + + differentiated_error = (error_value - self.last_value) / time_delta + differential = differentiated_error * self.differential_k + + output = proportional + integral + differential + # clamp output + output = min(self.max_output, output) + output = max(self.min_output, output) + + return output diff --git a/ch-10/2-graphing/pio_encoder.py b/ch-10/2-graphing/pio_encoder.py new file mode 100644 index 0000000..e9202f8 --- /dev/null +++ b/ch-10/2-graphing/pio_encoder.py @@ -0,0 +1,77 @@ +import rp2pio +import adafruit_pioasm +import array + +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]) + + 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] + \ No newline at end of file diff --git a/ch-10/2-graphing/robot.py b/ch-10/2-graphing/robot.py new file mode 100755 index 0000000..1b339ef --- /dev/null +++ b/ch-10/2-graphing/robot.py @@ -0,0 +1,50 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x + + +motor_A1 = pwmio.PWMOut(board.GP17) +motor_A2 = pwmio.PWMOut(board.GP16) +motor_B1 = pwmio.PWMOut(board.GP18) +motor_B2 = pwmio.PWMOut(board.GP19) + +right_motor = motor_A1, motor_A2 +left_motor = motor_B1, motor_B2 + +right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True) +left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27) + +i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3) + +right_distance = adafruit_vl53l1x.VL53L1X(i2c0) +left_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) diff --git a/ch-10/2-graphing/robot_wifi.py b/ch-10/2-graphing/robot_wifi.py new file mode 100644 index 0000000..1176507 --- /dev/null +++ b/ch-10/2-graphing/robot_wifi.py @@ -0,0 +1,25 @@ +import board +import busio +from digitalio import DigitalInOut +from adafruit_esp32spi import adafruit_esp32spi +from adafruit_esp32spi import adafruit_esp32spi_wifimanager + +try: + from secrets import secrets +except ImportError: + print("WiFi secrets are kept in secrets.py, please add them there!") + raise + + +def connect_to_wifi(): + esp32_cs = DigitalInOut(board.GP10) + esp32_ready = DigitalInOut(board.GP9) + esp32_reset = DigitalInOut(board.GP8) + + spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12) + esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset) + esp.reset() + wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets) + wifi.connect() + + return wifi, esp diff --git a/ch-10/3-wall-follow/pc/live_graph.py b/ch-10/3-wall-follow/pc/live_graph.py new file mode 100644 index 0000000..66670f2 --- /dev/null +++ b/ch-10/3-wall-follow/pc/live_graph.py @@ -0,0 +1,52 @@ +""" Turn JSON data stream into graphs""" +from itertools import count + +import requests + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + +url = 'http://192.168.1.128' + + +class SensorStream: + def __init__(self) -> None: + self.index = count() + self.error_values = [] + self.sensor_values = [] + self.pid_outputs = [] + self.x_values = [] + + self.set_point = requests.get(f"{url}/set_point").json() + + def sensor_stream(self): + item = requests.get(url).json() + print(f"Received: {item.decode('utf-8')}") + self.x_vals.append(next(self.index)) + self.error_values.append(item['error_value']) + self.sensor_values.append(item['sensor_value']) + self.pid_outputs.append(item['pid_output']) + + if len(self.x_vals) > 100: + self.x_vals = self.x_vals[-100:] + + self.error_values = self.error_values[-100:] + self.sensor_values = self.sensor_values[-100:] + self.pid_outputs = self.pid_outputs[-100:] + + plt.cla() # clear axes. + # plot the items + plt.plot(self.x_vals, self.error_values, label="error") + plt.plot(self.x_vals, self.sensor_values, label="sensor") + plt.plot(self.x_vals, self.pid_outputs, label="pid") + + plt.legend(loc='upper right') + + def start(self): + plt.style.use('fivethirtyeight') + # Create the animation. GFC - get current figure. random_stream - callback func. + self.ani = FuncAnimation(plt.gcf(), self.sensor_stream, interval=200) + plt.tight_layout() + plt.show() + +SensorStream().start() diff --git a/ch-10/3-wall-follow/pc/requirements.txt b/ch-10/3-wall-follow/pc/requirements.txt new file mode 100644 index 0000000..e69de29 diff --git a/ch-10/3-wall-follow/pid.py b/ch-10/3-wall-follow/pid.py new file mode 100644 index 0000000..3d5c300 --- /dev/null +++ b/ch-10/3-wall-follow/pid.py @@ -0,0 +1,19 @@ +class PID: + def __init__(self, proportional_k, integral_k, differential_k) -> None: + self.proportional_k = proportional_k + self.integral_k = integral_k + self.differential_k = differential_k + + self.integral = 0 + self.last_value = 0 + + def update(self, error_value, time_delta): + proportional = error_value * self.proportional_k + + self.integral += error_value * time_delta + integral = self.integral * self.integral_k + + differentiated_error = (error_value - self.last_value) / time_delta + differential = differentiated_error * self.differential_k + + return proportional + integral + differential diff --git a/ch-10/3-wall-follow/pio_encoder.py b/ch-10/3-wall-follow/pio_encoder.py new file mode 100644 index 0000000..e9202f8 --- /dev/null +++ b/ch-10/3-wall-follow/pio_encoder.py @@ -0,0 +1,77 @@ +import rp2pio +import adafruit_pioasm +import array + +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]) + + 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] + \ No newline at end of file diff --git a/ch-10/3-wall-follow/robot.py b/ch-10/3-wall-follow/robot.py new file mode 100755 index 0000000..1b339ef --- /dev/null +++ b/ch-10/3-wall-follow/robot.py @@ -0,0 +1,50 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x + + +motor_A1 = pwmio.PWMOut(board.GP17) +motor_A2 = pwmio.PWMOut(board.GP16) +motor_B1 = pwmio.PWMOut(board.GP18) +motor_B2 = pwmio.PWMOut(board.GP19) + +right_motor = motor_A1, motor_A2 +left_motor = motor_B1, motor_B2 + +right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True) +left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27) + +i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3) + +right_distance = adafruit_vl53l1x.VL53L1X(i2c0) +left_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) diff --git a/ch-10/3-wall-follow/robot_wifi.py b/ch-10/3-wall-follow/robot_wifi.py new file mode 100644 index 0000000..1176507 --- /dev/null +++ b/ch-10/3-wall-follow/robot_wifi.py @@ -0,0 +1,25 @@ +import board +import busio +from digitalio import DigitalInOut +from adafruit_esp32spi import adafruit_esp32spi +from adafruit_esp32spi import adafruit_esp32spi_wifimanager + +try: + from secrets import secrets +except ImportError: + print("WiFi secrets are kept in secrets.py, please add them there!") + raise + + +def connect_to_wifi(): + esp32_cs = DigitalInOut(board.GP10) + esp32_ready = DigitalInOut(board.GP9) + esp32_reset = DigitalInOut(board.GP8) + + spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12) + esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset) + esp.reset() + wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets) + wifi.connect() + + return wifi, esp diff --git a/ch-10/3-wall-follow/wall_follow.py b/ch-10/3-wall-follow/wall_follow.py new file mode 100644 index 0000000..ac06d32 --- /dev/null +++ b/ch-10/3-wall-follow/wall_follow.py @@ -0,0 +1,115 @@ +import time +import json +import math + +from adafruit_esp32spi import adafruit_esp32spi_wsgiserver +from adafruit_wsgi.wsgi_app import WSGIApp + +import pid +import robot +import robot_wifi + + +app = WSGIApp() + +class FollowWallApp: + def __init__(self) -> None: + self. self.speed = 0.6 + = 0.6 + self.max_deflection = 0.4 + + self.set_point = 15 + self.follow_pid = pid.PID(0.1, 0, 0) + self.wifi = None + self.server = None + + self.last_time = 0 + self.left_dist = 0 + self.error_value = 0 + self.pid_output = 0 + + def setup_robot(): + robot.left_distance.distance_mode = 1 + + def setup_wifi(self): + print("Setting up wifi.") + self.wifi, esp = robot_wifi.connect_to_wifi() + self.server = adafruit_esp32spi_wsgiserver.WSGIServer( + 80, + application=app + ) + adafruit_esp32spi_wsgiserver.set_interface(esp) + print("Starting server") + + self.server.start() + ip_int = ".".join(str(int(n)) for n in esp.ip_address) + print(f"IP Address is {ip_int}") + + @app.route("/") + def index(self, request): + return 200, [('Content-Type', 'application/json')], [json.dumps( + { + "error_value": self.error_value, + "left_dist": self.left_dist, + "pid_output": self.pid_output, + "last_time": self.last_time + } + )] + + @app.route("/set_point") + def set_point(self, request): + return 200, [('Content-Type', 'application/json')], [json.dumps(self.set_point)] + + def movement_update(self): + # do we have data + if robot.left_distance.data_ready: + self.left_dist = robot.left_distance.distance + + # get error value + self.error_value = self.left_dist - self.set_point + + # calculate time delta + new_time = time.monotonic() + time_delta = new_time - self.last_time + self.last_time = new_time + + # get turn from pid + self.pid_output = self.follow_pid.update(self.error_value, time_delta) + deflection = min(self.max_deflection, self.pid_output) + deflection = max(-self.max_deflection, deflection) + + # make movements + print(f"Dist: {self.left_dist}, Err: {self.error_value}, Deflection: {deflection}") + robot.set_left(self.speed + deflection) + robot.set_right(self.speed - deflection) + + # reset and loop + robot.left_distance.clear_interrupt() + + def main_loop(self): + robot.left_distance.start_ranging() + self.last_time = time.monotonic() + while True: + try: + self.movement_update() + self.server.update_poll() + time.sleep(0.1) + except RuntimeError as e: + print(f"Server poll error: {type(e)}, {e}") + robot.stop() + print(f"Resetting ESP...") + self.wifi.reset() + print("Reset complete.") + + def start(self): + print("Starting") + try: + self.setup_robot() + self.setup_wifi() + self.main_loop() + finally: + robot.stop() + robot.left_distance.clear_interrupt() + robot.left_distance.stop_ranging() + +FollowWallApp().start()