add free ebook notification

- Indentation issues
- Inconsistencies

.gitignore

@@ -5,3 +5,4 @@
 *.FCStd1
 secrets.py
 libs
+.idea/

README.md

@@ -0,0 +1,93 @@
+# Robotics at Home with Raspberry Pi Pico
+
+<a href="https://www.packtpub.com/product/robotics-at-home-with-raspberry-pi-pico/9781803246079?utm_source=github&utm_medium=repository&utm_campaign=9781803246079"><img src="https://static.packt-cdn.com/products/9781803246079/cover/smaller" alt="" height="256px" align="right"></a>
+
+This is the code repository for [Robotics at Home with Raspberry Pi Pico](https://www.packtpub.com/product/robotics-at-home-with-raspberry-pi-pico/9781803246079?utm_source=github&utm_medium=repository&utm_campaign=9781803246079), published by Packt.
+
+**Build autonomous robots with the versatile low-cost Raspberry Pi Pico controller and Python**
+
+## What is this book about?
+The field of robotics is expanding, and this is the perfect time to learn how to create robots at home for different purposes. This book will help you take your first steps in planning, building, and programming a robot with Raspberry Pi Pico, an impressive controller bursting with IO capabilities. After a quick tour of the Pico, you'll begin designing a robot chassis in 3D CAD. With easy-to-follow instructions, shopping lists, and plans, you'll start building the robot. Further, you'll add simple sensors and outputs to extend the robot, reinforce design skills and build knowledge in programming with circuit Python, understand interactions with electronics, standard robotics algorithms, and the discipline and process for building robots. Moving forward, you'll learn to add more complicated sensors and robotic behaviors, with increasing complexity levels, giving you hands-on experience. You'll learn Raspberry Pi Pico's excellent features like PIO, adding capabilities like avoiding walls, detecting movement and compass heading. You'll combine these with Bluetooth BLE for seeing sensor data and remotely control with a smartphone. Finally, you'll program the robot to find its location in an arena.
+
+This book covers the following exciting features:
+* Interface Raspberry Pi Pico with motors to move parts
+* Design in 3D CAD with Free CAD
+* Build a simple robot and extend it for more complex projects
+* Interface Raspberry Pi Pico with sensors and Bluetooth BLE
+* Visualize robot data with Matplotlib
+* Gain an understanding of robotics algorithms on Pico for smart behavior
+
+If you feel this book is for you, get your [copy](https://www.amazon.com/dp/1803246073) today!
+
+<a href="https://www.packtpub.com/?utm_source=github&utm_medium=banner&utm_campaign=GitHubBanner"><img src="https://raw.githubusercontent.com/PacktPublishing/GitHub/master/GitHub.png" 
+alt="https://www.packtpub.com/" border="5" /></a>
+
+## Instructions and Navigations
+All of the code is organized into folders. For example, ch-01.
+
+A block of code is set as follows:
+```
+import time
+import board
+import digitalio
+
+led = digitalio.DigitalInOut(board.LED)
+led.direction = digitalio.Direction.OUTPUT
+
+while True:
+  led.value = True
+  time.sleep(0.5)
+  led.value = False
+  time.sleep(0.5)
+```
+When we wish to draw your attention to a particular part of a code block, the relevant lines or items are set in bold:
+```
+>>> 𝗽𝗿𝗶𝗻𝘁("𝗛𝗲𝗹𝗹𝗼, 𝘄𝗼𝗿𝗹𝗱!")
+Hello, World!
+>>>
+```
+Any command-line input or output is written as follows:
+```
+𝗰𝗼𝗱𝗲.𝗽𝘆 𝗼𝘂𝘁𝗽𝘂𝘁:
+𝟰𝟰𝟰𝟯 𝟰𝟱𝟮𝟮
+```
+
+**Following is what you need for this book:**
+This book is for beginner robot makers, keen hobbyists, technical enthusiasts, developers and STEM teachers who want to build robots at home. Prior knowledge of coding - beginner to intermediate programming, will be helpful.
+
+With the following software and hardware list you can run all code files present in the book (Chapter 1-14).
+### Software and Hardware List
+| Software required | OS required |
+| ------------------------------------ | ----------------------------------- |
+| Thonny > 3.3 or Mu Editor > 1.1 | macOS, Linux, or Windows |
+| Python 3.7 or later | macOS, Linux, or Windows |
+| Matplotlib 3.6.1 or later | macOS, Linux, or Windows |
+| NumPy 1.23.4 or later | macOS, Linux, or Windows |
+| Bleak (Python BLE library) 0.19.0 or above | macOS, Linux, or Windows |
+| Free USB port | macOS, Linux, or Windows |
+| Smartphone/tablet with Bluetooth LE (Bluetooth > 4.0) | iOS or Android |
+| Adafruit Bluefruit LE Connect > 3.3.2 | iOS or Android |
+| Bluetooth LE-enabled laptop (or BLE dongle) | macOS, Linux, or Windows |
+| FreeCAD | macOS, Linux, or Windows |
+| Raspberry Pi Pico |  |
+| CircuitPython > 7.2.0 | Raspberry Pi Pico |
+
+We also provide a PDF file that has color images of the screenshots/diagrams used in this book. [Click here to download it](https://packt.link/7x3ku).
+
+### Related products
+* Raspberry Pi Pico DIY Workshop [[Packt]](https://www.packtpub.com/product/raspberry-pi-pico-diy-workshop/9781801814812?utm_source=github&utm_medium=repository&utm_campaign=9781801814812) [[Amazon]](https://www.amazon.com/dp/1801814813)
+
+* Learn Robotics Programming - Second Edition [[Packt]](https://www.packtpub.com/product/learn-robotics-programming-second-edition/9781839218804?utm_source=github&utm_medium=repository&utm_campaign=9781839218804) [[Amazon]](https://www.amazon.com/dp/1839218800)
+
+## Get to Know the Author
+**Danny Staple**
+Danny Staple is an author who builds robots and gadgets at home, and makes videos about his work on the YouTube channel OrionRobots. He attends robotics and maker events such as PiWars and Arduino Day. He's been building robots for 20 years, having run Lego Robotics clubs with Mindstorms and is a mentor at a local CoderDojo, where he teaches kids how to code with Python.
+He has been a professional Python programmer since 2009, and a software engineer since 2000. He has worked with embedded systems, throughout the majority of his career. The robots he has built at home with his children include TankBot, SkittleBot (a PiWars 2018 robot), Bangers n Bash (LunchBot), ArmBot, and SpiderBot. He is better at building robots than naming them.
+
+## Other books by the authors
+* [Learn Robotics Programming - Second Edition](https://www.packtpub.com/product/learn-robotics-programming-second-edition/9781839218804?utm_source=github&utm_medium=repository&utm_campaign=9781839218804)
+
+### Download a free PDF
+
+ <i>If you have already purchased a print or Kindle version of this book, you can get a DRM-free PDF version at no cost.<br>Simply click on the link to claim your free PDF.</i>
+<p align="center"> <a href="https://packt.link/free-ebook/9781803246079">https://packt.link/free-ebook/9781803246079 </a> </p>

ch-10/1-proportional-distance-control/code.py

@@ -14,22 +14,22 @@ class PController:
 
 
 ## We'll set up a single distance sensor, and keep a set distance from an object
-robot.left_distance.distance_mode = 1
-robot.left_distance.start_ranging()
+robot.right_distance.distance_mode = 1
+robot.right_distance.start_ranging()
 
 distance_set_point = 10
 distance_controller = PController(-0.1)
 
 while True:
-  if robot.left_distance.data_ready:
-    distance = robot.left_distance.distance
+  if robot.right_distance.data_ready:
+    distance = robot.right_distance.distance
     error = distance_set_point - distance
     speed = distance_controller.calculate(error)
-    if abs(speed) < 0.2:
+    if abs(speed) < 0.3:
       speed = 0
     uart.write(f"{error},{speed}\n".encode())
     print(f"{error},{speed}")
     robot.set_left(speed)
     robot.set_right(speed)
-    robot.left_distance.clear_interrupt()
+    robot.right_distance.clear_interrupt()
     time.sleep(0.05)

ch-10/2-pi-distance-control/code.py

@@ -17,28 +17,28 @@ class PIController:
 
 
 ## We'll set up a single distance sensor, and keep a set distance from an object
-robot.left_distance.distance_mode = 1
-robot.left_distance.start_ranging()
+robot.right_distance.distance_mode = 1
+robot.right_distance.start_ranging()
 
 distance_set_point = 10
 distance_controller = PIController(-0.19, -0.005)
 
 prev_time = time.monotonic()
 while True:
-  if robot.left_distance.data_ready:
-    distance = robot.left_distance.distance
+  if robot.right_distance.data_ready:
+    distance = robot.right_distance.distance
     error = distance_set_point - distance
 
     current_time = time.monotonic()
     speed = distance_controller.calculate(error, current_time - prev_time)
     prev_time = current_time
     # Control the motors with the speed
-    if abs(speed) < 0.35:
+    if abs(speed) < 0.3:
       speed = 0
     uart.write(f"{error},{speed},"
       f"{distance_controller.integral}\n".encode())
     print(f"{error},{speed},{distance_controller.integral}")
     robot.set_left(speed)
     robot.set_right(speed)
-    robot.left_distance.clear_interrupt()
+    robot.right_distance.clear_interrupt()
     time.sleep(0.05)

ch-10/3-damping-with-derivative/code.py

@@ -8,28 +8,28 @@ uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
 
 ## We'll set up a single distance sensor, and keep a set distance from an object
-robot.left_distance.distance_mode = 1
-robot.left_distance.start_ranging()
+robot.right_distance.distance_mode = 1
+robot.right_distance.start_ranging()
 
 distance_set_point = 10
 distance_controller = PIDController(-0.09, -0.02, -0.07)
 
 prev_time = time.monotonic()
 while True:
-  if robot.left_distance.data_ready:
-    distance = robot.left_distance.distance
+  if robot.right_distance.data_ready:
+    distance = robot.right_distance.distance
     error = distance_set_point - distance
 
     current_time = time.monotonic()
     speed = distance_controller.calculate(error, current_time - prev_time)
     prev_time = current_time
     # Control the motors with the speed
-    if abs(speed) < 0.35:
+    if abs(speed) < 0.3:
       speed = 0
     uart.write(f"{error},{speed},{distance_controller.integral},{distance_controller.derivative}\n".encode())
     print(f"{error},{speed},{distance_controller.integral},{distance_controller.derivative}")
     robot.set_left(speed)
     robot.set_right(speed)
     # reset the distance sensor
-    robot.left_distance.clear_interrupt()
+    robot.right_distance.clear_interrupt()
     time.sleep(0.05)

ch-10/4-wall-follow/code.py

@@ -25,7 +25,8 @@ while True:
     current_time = time.monotonic()
     deflection = distance_controller.calculate(error, current_time - prev_time)
     prev_time = current_time
-    uart.write(f"{error},{deflection}\n".encode()) # ,{distance_controller.derivative}
+    uart.write(f"{error},{deflection},"
+      f"{distance_controller.derivative}\n".encode())
     if motors_active:
       robot.set_left(speed - deflection)
       robot.set_right(speed + deflection)

ch-11/1-encoder-speed/encoder_speed.py

@@ -1,73 +0,0 @@
-import time
-import json
-
-from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
-from adafruit_wsgi.wsgi_app import WSGIApp
-
-import robot
-import robot_wifi
-
-
-class SpeedCountApp:
-    def __init__(self):
-        self.intended_speed = 0.9
-        self.last_time = time.monotonic()
-        self.wifi = None
-        self.server = None
-
-    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):
-        new_time = time.monotonic()
-        time_delta = new_time - self.last_time
-        self.last_time = new_time
-
-        left_speed = robot.left_encoder.get_speed(time_delta)
-        right_speed = robot.right_encoder.get_speed(time_delta)
-        return (
-            200,
-            [("Content-Type", "application/json")],
-            [
-                json.dumps(
-                    {
-                        "left_speed": left_speed,
-                        "right_speed": right_speed,
-                        "time": self.last_time,
-                    }
-                )
-            ],
-        )
-
-    def main_loop(self):
-        robot.set_left(self.intended_speed)
-        robot.set_right(self.intended_speed)
-        while True:
-            try:
-                self.server.update_poll()
-            except RuntimeError as e:
-                print(f"Server poll error: {type(e)}, {e}")
-                print(f"Resetting ESP...")
-                self.wifi.reset()
-                print("Reset complete.")
-
-    def start(self):
-        app = WSGIApp()
-        app.route("/")(self.index)
-        print("Starting")
-        try:
-            self.setup_wifi(app)
-            self.main_loop()
-        finally:
-            robot.stop()
-
-
-SpeedCountApp().start()

ch-11/1-encoder-speed/robot_wifi.py

@@ -1,25 +0,0 @@
-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-11/1-measuring-wheel-speeds/code.py

@@ -0,0 +1,47 @@
+import asyncio
+import robot
+
+class Settings:
+  speed = 0.7
+  time_interval = 0.2
+
+async def motor_speed_loop():
+  left_last, right_last = robot.left_encoder.read(), robot.right_encoder.read()
+  while True:
+    await asyncio.sleep(Settings.time_interval)
+    left_new, right_new = robot.left_encoder.read(), robot.right_encoder.read()
+    left_speed = robot.ticks_to_m * (left_new - left_last) / Settings.time_interval
+    left_last = left_new
+    right_speed = robot.ticks_to_m * (right_new - right_last) / Settings.time_interval
+    right_last = right_new
+    robot.send_line(f"{left_speed:.2f},{right_speed:.2f},0")
+
+async def stop_motors_after(seconds):
+  await asyncio.sleep(seconds)
+  robot.stop()
+
+async def command_handler():
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      if command.startswith("M"):
+        Settings.speed = float(command[1:])
+      elif command.startswith("T"):
+        Settings.time_interval = float(command[1:])
+      elif command == "G":
+        robot.stop()
+      elif command.startswith("G"):
+        await asyncio.sleep(5)
+        asyncio.create_task(
+          stop_motors_after(float(command[1:]))
+        )
+        robot.set_left(Settings.speed)
+        robot.set_right(Settings.speed)
+      elif command.startswith("?"):
+        robot.send_line(f"M{Settings.speed:.1f}")
+        robot.send_line(f"T{Settings.time_interval:.1f}")
+        await asyncio.sleep(3)
+    await asyncio.sleep(0)
+
+asyncio.create_task(motor_speed_loop())
+asyncio.run(command_handler())

ch-11/1-measuring-wheel-speeds/pid_controller.py

@@ -0,0 +1,20 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+
+        # 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-measuring-wheel-speeds/pio_encoder.py

@@ -1,6 +1,8 @@
 import rp2pio
 import adafruit_pioasm
 import array
+import asyncio
+
 
 program = """
 ; use the osr for count
@@ -67,18 +69,16 @@ class QuadratureEncoder:
         )
         self.reversed = reversed
         self._buffer = array.array("i", [0])
-        self.previous_reading = 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):
-        while self.sm.in_waiting:
-            self.sm.readinto(self._buffer)
         if self.reversed:
             return -self._buffer[0]
         else:
             return self._buffer[0]
-
-    def get_speed(self, delta_time):
-        new_read = self.read()
-        distance = new_read - self.previous_reading
-        self.previous_reading = new_read
-        return distance / delta_time

ch-11/1-measuring-wheel-speeds/robot.py

@@ -3,18 +3,27 @@ import pwmio
 import pio_encoder
 import busio
 import adafruit_vl53l1x
+import math
 
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
-motor_A1 = pwmio.PWMOut(board.GP17)
-motor_A2 = pwmio.PWMOut(board.GP16)
-motor_B1 = pwmio.PWMOut(board.GP18)
-motor_B2 = pwmio.PWMOut(board.GP19)
+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_m = (wheel_circumference_mm / ticks_per_revolution) / 1000
+
+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, reversed=True)
-left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
+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)
@@ -50,3 +59,6 @@ def set_left(speed):
 
 def set_right(speed):
     set_speed(right_motor, speed)
+
+def send_line(message):
+    uart.write(f"{message}\n".encode())

ch-11/2-speed-control/code.py

@@ -0,0 +1,88 @@
+import asyncio
+import time
+import robot
+import pid_controller
+
+class Settings:
+  speed = 0.17
+  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.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
+    self.actual_speed = robot.ticks_to_m * speed_in_ticks
+    # calculate the error
+    error = Settings.speed - self.actual_speed
+    # calculate the control signal
+    control_signal = self.pid.calculate(error, dt)
+    self.pwm += control_signal
+    self.motor_fn(self.pwm * Settings.motors_enabled)
+
+
+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.send_line(f"{left.actual_speed:.2f},{Settings.speed * Settings.motors_enabled:.2f},0")
+
+
+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()
+      if command.startswith("M"):
+        Settings.speed = float(command[1:])
+      elif command.startswith("T"):
+        Settings.time_interval = float(command[1:])
+      elif command == "G":
+        Settings.motors_enabled = False
+      elif command.startswith("G"):
+        await asyncio.sleep(5)
+        asyncio.create_task(
+          stop_motors_after(float(command[1:]))
+        )
+        Settings.motors_enabled = True
+        left.reset()
+        right.reset()
+      elif command.startswith("?"):
+        robot.send_line(f"M{Settings.speed:.1f}")
+        robot.send_line(f"T{Settings.time_interval:.1f}")
+        await asyncio.sleep(3)
+    await asyncio.sleep(0)
+
+try:
+  motors_task = asyncio.create_task(motor_speed_loop())
+  asyncio.run(command_handler())
+finally:
+  motors_task.cancel()
+  robot.stop()

ch-11/2-speed-control/pid_controller.py

@@ -0,0 +1,21 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+        self.reset()
+
+    def reset(self):
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+        # 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/pio_encoder.py

@@ -1,6 +1,8 @@
 import rp2pio
 import adafruit_pioasm
 import array
+import asyncio
+
 
 program = """
 ; use the osr for count
@@ -67,18 +69,16 @@ class QuadratureEncoder:
         )
         self.reversed = reversed
         self._buffer = array.array("i", [0])
-        self.previous_reading = 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):
-        while self.sm.in_waiting:
-            self.sm.readinto(self._buffer)
         if self.reversed:
             return -self._buffer[0]
         else:
             return self._buffer[0]
-
-    def get_speed(self, delta_time):
-        new_read = self.read()
-        distance = new_read - self.previous_reading
-        self.previous_reading = new_read
-        return distance / delta_time

ch-11/2-speed-control/robot.py

@@ -3,27 +3,34 @@ import pwmio
 import pio_encoder
 import busio
 import adafruit_vl53l1x
-import adafruit_bno055
+import math
 
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
-motor_A1 = pwmio.PWMOut(board.GP17)
-motor_A2 = pwmio.PWMOut(board.GP16)
-motor_B1 = pwmio.PWMOut(board.GP18)
-motor_B2 = pwmio.PWMOut(board.GP19)
+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_m = (wheel_circumference_mm / ticks_per_revolution) / 1000
+
+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, reversed=True)
-left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
+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)
+i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
 
-# right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
-# left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
+left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
+right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
 
-imu = adafruit_bno055.BNO055_I2C(i2c0)
 
 def stop():
     motor_A1.duty_cycle = 0
@@ -34,6 +41,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
@@ -52,3 +63,6 @@ def set_left(speed):
 
 def set_right(speed):
     set_speed(right_motor, speed)
+
+def send_line(message):
+    uart.write(f"{message}\n".encode())

ch-11/2-straight-line-speed/pid.py

@@ -1,39 +0,0 @@
-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
-
-        self.dead_zone = 0.3
-
-    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
-        self.last_value = error_value
-
-        output = proportional + integral + differential
-        # clamp output
-        if abs(output) < self.dead_zone:
-            output = 0
-        else:
-            output = min(self.max_output, output)
-            output = max(self.min_output, output)
-
-        return output

ch-11/2-straight-line-speed/robot_wifi.py

@@ -1,25 +0,0 @@
-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-11/2-straight-line-speed/speed_control.py

@@ -1,109 +0,0 @@
-import time
-import json
-
-from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
-from adafruit_wsgi.wsgi_app import WSGIApp
-
-import robot
-import robot_wifi
-import pid
-
-
-class SpeedControlApp:
-    def __init__(self):
-        self.wifi = None
-        self.server = None
-
-        self.intended_speed = 0.9
-        self.last_time = time.monotonic()
-        self.speed_to_encoder_factor = 1/5100
-
-        self.left_speed_pid = pid.PID(0, -0.7, 0, self.intended_speed)
-        self.right_speed_pid = pid.PID(0, -0.7, 0, self.intended_speed)
-        self.left_pid_output = 0
-        self.right_pid_output = 0
-        self.left_speed = 0
-        self.right_speed = 0
-
-
-    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 update(self):
-        new_time = time.monotonic()
-        time_delta = new_time - self.last_time
-        self.last_time = new_time
-
-        self.left_speed = robot.left_encoder.get_speed(time_delta) * self.speed_to_encoder_factor
-        self.right_speed = robot.right_encoder.get_speed(time_delta) * self.speed_to_encoder_factor
-
-        self.left_pid_output = self.left_speed_pid.update(self.left_speed, time_delta)
-        self.right_pid_output = self.right_speed_pid.update(self.right_speed, time_delta)
-
-        # print({
-        #         "left_speed": self.left_speed,
-        #         "left_pid": self.left_pid_output,
-        #         "right_speed": self.right_speed,
-        #         "right_pid": self.right_pid_output,
-        #         "time": self.last_time,
-        #         "error_sum": self.left_speed_pid.error_sum
-        #     })
-
-        # robot.set_left(self.left_pid_output)
-        # robot.set_right(self.right_pid_output)
-
-    def movement_generator(self):
-        while True:
-            self.update()
-            data = json.dumps(
-                    {
-                        "left_speed": self.left_speed,
-                        "left_pid": self.left_pid_output,
-                        "right_speed": self.right_speed,
-                        "right_pid": self.right_pid_output,
-                        "time": self.last_time,
-                    }
-                ) + "/n"
-            print(data)
-            yield data
-
-    def index(self, request):
-        return (
-            200,
-            [("Content-Type", "application/json")],
-            self.movement_generator(),
-        )
-
-    def main_loop(self):
-        while True:
-            try:
-                self.update()
-
-                # time.sleep(0.1)
-                self.server.update_poll()
-            except RuntimeError as e:
-                print(f"Server poll error: {type(e)}, {e}")
-                print(f"Resetting ESP...")
-                self.wifi.reset()
-                print("Reset complete.")
-
-    def start(self):
-        app = WSGIApp()
-        app.route("/")(self.index)
-        print("Starting")
-        try:
-            self.setup_wifi(app)
-            self.main_loop()
-        finally:
-            robot.stop()
-
-
-SpeedControlApp().start()

ch-11/3-known-distance/code.py

@@ -0,0 +1,84 @@
+import asyncio
+import time
+import robot
+import pid_controller
+
+
+class DistanceController:
+  def __init__(self, encoder, motor_fn):
+    self.encoder = encoder
+    self.motor_fn = motor_fn
+    self.pid = pid_controller.PIDController(3.25, 0.5,	0.5, d_filter_gain=1) 
+    self.start_ticks = self.encoder.read()
+    self.error = 0
+
+  def update(self, dt, expected):
+    self.actual = self.encoder.read() - self.start_ticks
+    # calculate the error
+    self.error = (expected - self.actual) / robot.ticks_per_revolution
+    # calculate the control signal
+    control_signal = self.pid.calculate(self.error, dt)
+    self.motor_fn(control_signal)
+ 
+class DistanceTracker:
+  def __init__(self):
+    self.speed = 0.17
+    self.time_interval = 0.2
+    self.start_time = time.monotonic()
+    self.current_position = 0
+    self.total_distance_in_ticks = 0
+    self.total_time = 0.1
+
+  def set_distance(self, new_distance):
+    # add the last travelled distance to the current position
+    self.current_position += self.total_distance_in_ticks
+    # calculate the new additional distance
+    self.total_distance_in_ticks = robot.m_to_ticks * new_distance
+    self.total_time = max(0.1, abs(new_distance / self.speed))
+    self.start_time = time.monotonic()
+
+  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 + self.current_position
+      left.update(dt, expected)
+      right.update(dt, expected)
+      robot.send_line(f"{expected:.2f},{left.actual:.2f},0")
+
+
+distance_tracker = DistanceTracker()
+
+
+async def command_handler():
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      if command.startswith("M"):
+        distance_tracker.speed = float(command[1:])
+      elif command.startswith("T"):
+        distance_tracker.time_interval = float(command[1:])
+      elif command == "G":
+        distance_tracker.set_distance(0)
+      elif command.startswith("G"):
+        await asyncio.sleep(5)
+        distance_tracker.set_distance(float(command[1:]))
+      elif command.startswith("?"):
+        robot.send_line(f"M{distance_tracker.speed:.1f}")
+        robot.send_line(f"T{distance_tracker.time_interval:.1f}")
+        await asyncio.sleep(3)
+    await asyncio.sleep(0)
+    
+try:
+  motors_task = asyncio.create_task(distance_tracker.loop())
+  asyncio.run(command_handler())
+finally:
+  motors_task.cancel()
+  robot.stop()

ch-11/3-known-distance/pid_controller.py

@@ -0,0 +1,21 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+        self.reset()
+
+    def reset(self):
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+        # 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/3-known-distance/pio_encoder.py

@@ -0,0 +1,84 @@
+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/3-known-distance/robot.py

@@ -0,0 +1,70 @@
+import board
+import pwmio
+import pio_encoder
+import busio
+import adafruit_vl53l1x
+import math
+
+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_m = (wheel_circumference_mm / ticks_per_revolution) / 1000
+m_to_ticks = 1 / ticks_to_m
+
+
+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 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
+    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)
+
+def send_line(message):
+    uart.write(f"{message}\n".encode())

ch-11/_test_encoder_output/code.py

@@ -0,0 +1,34 @@
+import time
+import board
+import busio
+import robot
+
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
+
+speed = 0.7
+stop_time = 0
+
+while True:
+  current_time = time.monotonic()
+  if current_time > stop_time:
+    robot.set_left(0)
+    robot.set_right(0)
+  left_value = robot.left_encoder.read()
+  right_value = robot.right_encoder.read()
+  uart.write(f"{left_value:.3f},{right_value:.3f}\n".encode() )
+  time.sleep(0.02)
+  if uart.in_waiting:
+    command = uart.readline().decode().strip()
+    if command.startswith("M"):
+      speed = float(command[1:])
+    elif command == "O":
+      stop_time = 0
+      robot.set_left(0)
+      robot.set_right(0)
+    elif command.startswith("O"):
+      stop_time = float(command[1:]) + current_time
+      robot.set_left(speed)
+      robot.set_right(speed)
+    elif command.startswith("?"):
+      uart.write(f"M{speed:.1f}\n".encode())
+      time.sleep(3)

ch-11/_test_encoder_output/lpf.py

@@ -0,0 +1,8 @@
+class LPF:
+  """Low pass filter."""
+  def __init__(self, alpha):
+    self.alpha = alpha
+    self.last = 0
+  def update(self, value):
+    self.last = self.alpha * value + (1 - self.alpha) * self.last
+    return self.last

ch-11/_test_encoder_output/pid_controller.py

@@ -0,0 +1,20 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+
+        # 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