Chapter 10 follow behaviors

.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

.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

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()

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

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]
+ 

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)

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()

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()

ch-10/2-graphing/pc/requirements.txt

@@ -0,0 +1,2 @@
+matplotlib
+requests

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

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]
+ 

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)

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

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()

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

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]
+ 

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)

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

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()