add free ebook notification

- Indentation issues
- Inconsistencies

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-measuring-wheel-speeds/code.py

@@ -10,11 +10,11 @@ async def motor_speed_loop():
   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_mm(left_new - left_last) / Settings.time_interval
+    left_speed = robot.ticks_to_m * (left_new - left_last) / Settings.time_interval
     left_last = left_new
-    right_speed = robot.ticks_to_mm(right_new - right_last) / Settings.time_interval
+    right_speed = robot.ticks_to_m * (right_new - right_last) / Settings.time_interval
     right_last = right_new
-    robot.uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode())
+    robot.send_line(f"{left_speed:.2f},{right_speed:.2f},0")
 
 async def stop_motors_after(seconds):
   await asyncio.sleep(seconds)
@@ -28,21 +28,20 @@ async def command_handler():
         Settings.speed = float(command[1:])
       elif command.startswith("T"):
         Settings.time_interval = float(command[1:])
-      elif command == "O":
+      elif command == "G":
         robot.stop()
-      elif command.startswith("O"):
+      elif command.startswith("G"):
         await asyncio.sleep(5)
-        asyncio.create_task(stop_motors_after(float(command[1:])))
+        asyncio.create_task(
+          stop_motors_after(float(command[1:]))
+        )
         robot.set_left(Settings.speed)
         robot.set_right(Settings.speed)
       elif command.startswith("?"):
-        robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
-        robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
+        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:
-  asyncio.create_task(motor_speed_loop())
-  asyncio.run(command_handler())
-finally:
-  robot.stop()
+asyncio.create_task(motor_speed_loop())
+asyncio.run(command_handler())

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

@@ -4,7 +4,6 @@ import pio_encoder
 import busio
 import adafruit_vl53l1x
 import math
-import busio
 
 uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
@@ -13,10 +12,7 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm
 gear_ratio = 298
 encoder_poles = 28
 ticks_per_revolution = encoder_poles * gear_ratio
-ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
-
-def ticks_to_mm(ticks):
-    return ticks_to_mm_const * ticks
+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)
@@ -63,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

@@ -26,13 +26,13 @@ class SpeedController:
     current_ticks = self.encoder.read()
     speed_in_ticks = (current_ticks - self.last_ticks) / dt
     self.last_ticks = current_ticks
-    self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
+    self.actual_speed = robot.ticks_to_m * speed_in_ticks
     # calculate the error
-    error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
+    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)
+    self.motor_fn(self.pwm * Settings.motors_enabled)
 
 
 left = SpeedController(robot.left_encoder, robot.set_left)
@@ -48,7 +48,7 @@ async def motor_speed_loop():
     last_time = current_time
     left.update(dt)
     right.update(dt)
-    robot.uart.write(f"0, {left.actual_speed:.2f},{Settings.speed * Settings.motors_enabled:.2f}\n".encode())
+    robot.send_line(f"{left.actual_speed:.2f},{Settings.speed * Settings.motors_enabled:.2f},0")
 
 
 async def stop_motors_after(seconds):
@@ -64,23 +64,25 @@ async def command_handler():
         Settings.speed = float(command[1:])
       elif command.startswith("T"):
         Settings.time_interval = float(command[1:])
-      elif command == "O":
+      elif command == "G":
         Settings.motors_enabled = False
-      elif command.startswith("O"):
+      elif command.startswith("G"):
         await asyncio.sleep(5)
-        asyncio.create_task(stop_motors_after(float(command[1:])))
+        asyncio.create_task(
+          stop_motors_after(float(command[1:]))
+        )
         Settings.motors_enabled = True
         left.reset()
         right.reset()
-      # Print settings
       elif command.startswith("?"):
-        robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
-        robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
+        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:
-  asyncio.create_task(motor_speed_loop())
+  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

@@ -1,11 +1,9 @@
 class PIDController:
-    def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
+    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.imax = imax
-        self.imin = imin
         self.reset()
 
     def reset(self):
@@ -15,10 +13,6 @@ class PIDController:
 
     def calculate(self, error, dt):
         self.integral += error * dt
-        if self.imax is not None and self.integral > self.imax:
-            self.integral = self.imax
-        if self.imin is not None and self.integral < self.imin:
-            self.integral = self.imin
         # Add a low pass filter to the difference
         difference = (error - self.error_prev) * self.d_filter_gain
         self.error_prev += difference

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

@@ -4,7 +4,6 @@ import pio_encoder
 import busio
 import adafruit_vl53l1x
 import math
-import busio
 
 uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
@@ -13,10 +12,7 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm
 gear_ratio = 298
 encoder_poles = 28
 ticks_per_revolution = encoder_poles * gear_ratio
-ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
-
-def ticks_to_mm(ticks):
-    return ticks_to_mm_const * ticks
+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)
@@ -67,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/3-known-distance/code.py

@@ -8,42 +8,34 @@ class DistanceController:
   def __init__(self, encoder, motor_fn):
     self.encoder = encoder
     self.motor_fn = motor_fn
-    # accel
-    # self.pid = pid_controller.PIDController(0.00000, 0,	0.00008, d_filter_gain=1) 
-    self.pid = pid_controller.PIDController(0.00000, 0.0000,	0.00001, d_filter_gain=1) 
+    self.pid = pid_controller.PIDController(3.25, 0.5,	0.5, d_filter_gain=1) 
     self.start_ticks = self.encoder.read()
-    self.pwm = 0
     self.error = 0
 
-  def update(self, dt, expected, debug=False):
-    actual = self.encoder.read() - self.start_ticks
+  def update(self, dt, expected):
+    self.actual = self.encoder.read() - self.start_ticks
     # calculate the error
-    self.error = expected - actual
-
+    self.error = (expected - self.actual) / robot.ticks_per_revolution
     # calculate the control signal
     control_signal = self.pid.calculate(self.error, dt)
-    print(control_signal)
-    # self.pwm += control_signal
-    if debug:
-      robot.uart.write(f"0, {expected:.2f},{actual:.2f}\n".encode())
-    # self.motor_fn(self.pwm)
     self.motor_fn(control_signal)
-
+ 
 class DistanceTracker:
   def __init__(self):
-    self.speed = 0.10
+    self.speed = 0.17
     self.time_interval = 0.2
-    self.reset()
-
-  def reset(self):
     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):
-    self.reset()
-    self.total_distance_in_ticks = robot.mm_to_ticks(new_distance * 1000)
-    self.total_time = new_distance / self.speed
+    # 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)
@@ -56,9 +48,10 @@ class DistanceTracker:
       last_time = current_time
       elapsed_time = current_time - self.start_time
       time_proportion = min(1, elapsed_time / self.total_time)
-      expected = time_proportion * self.total_distance_in_ticks
-      left.update(dt, expected, debug=True)
+      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()
@@ -68,26 +61,24 @@ async def command_handler():
   while True:
     if robot.uart.in_waiting:
       command = robot.uart.readline().decode().strip()
-      # PID settings
       if command.startswith("M"):
         distance_tracker.speed = float(command[1:])
       elif command.startswith("T"):
         distance_tracker.time_interval = float(command[1:])
-      # Start/stop commands
-      elif command == "O":
+      elif command == "G":
         distance_tracker.set_distance(0)
-      elif command.startswith("O"):
+      elif command.startswith("G"):
         await asyncio.sleep(5)
         distance_tracker.set_distance(float(command[1:]))
-      # Print settings
       elif command.startswith("?"):
-        robot.uart.write(f"M{distance_tracker.speed:.1f}\n".encode())
-        robot.uart.write(f"T{distance_tracker.time_interval:.1f}\n".encode())
+        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:
-  asyncio.create_task(distance_tracker.loop())
+  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/robot.py

@@ -4,7 +4,6 @@ import pio_encoder
 import busio
 import adafruit_vl53l1x
 import math
-import busio
 
 uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
@@ -13,13 +12,8 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm
 gear_ratio = 298
 encoder_poles = 28
 ticks_per_revolution = encoder_poles * gear_ratio
-ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
-
-def ticks_to_mm(ticks):
-    return ticks_to_mm_const * ticks
-
-def mm_to_ticks(mm):
-    return mm / ticks_to_mm_const
+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)
@@ -71,3 +65,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-12/1-connect/code.py

@@ -0,0 +1,8 @@
+import adafruit_bno055
+import board
+import busio
+
+i2c = busio.I2C(sda=board.GP0, scl=board.GP1)
+sensor = adafruit_bno055.BNO055_I2C(i2c)
+
+print("Temperature: {} degrees C".format(sensor.temperature))

ch-12/2-calibrate/code.py

@@ -0,0 +1,24 @@
+import adafruit_bno055
+import board
+import busio
+import time
+
+i2c = busio.I2C(sda=board.GP0, scl=board.GP1)
+imu = adafruit_bno055.BNO055_I2C(i2c)
+
+def check_status():
+  sys_status, gyro, accel, mag = imu.calibration_status
+  print(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}")
+  return sys_status == 3
+
+while not check_status():
+  time.sleep(0.1)
+
+while True:
+  data = {"temperature": imu.temperature, 
+          "acceleration": imu.acceleration, 
+          "magnetic": imu.magnetic, 
+          "gyro": imu.gyro,
+          "euler": imu.euler}
+  print(data)
+  time.sleep(0.1)

ch-12/3-always-face-north/code.py

@@ -0,0 +1,52 @@
+import robot
+import pid_controller
+import asyncio
+import time
+
+# PID Loop control for IMU face north
+class FaceNorthController:
+    def __init__(self):
+        self.pid = pid_controller.PIDController(0.01, 0.010, 0)
+        self.target = 0
+
+    def update(self, dt, angle):
+        error = self.target - angle
+        if error > 180:
+            error -= 360
+        elif error < -180:
+            error += 360
+        control_signal = self.pid.calculate(error, dt)
+        robot.set_left(control_signal)
+        robot.set_right(-control_signal)
+
+async def control_loop():
+  controller = FaceNorthController()
+  last_time = time.monotonic()
+  while True:
+    await asyncio.sleep(0.1)
+    next_time = time.monotonic()
+    dt = next_time - last_time
+    last_time = next_time
+    angle = robot.imu.euler[0]
+
+    controller.update(dt, angle)
+    robot.send_line(f"{angle}, 0")
+
+
+async def main():
+  while not robot.check_imu_status():
+    await asyncio.sleep(0.1)
+
+  robot.send_line("Ready to go!")
+  # Wait for start signal
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      if command == "start":
+        break
+    await asyncio.sleep(0.1)
+
+  await control_loop()
+
+print("Starting")
+asyncio.run(main())

ch-12/3-always-face-north/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-12/3-always-face-north/robot.py

@@ -0,0 +1,76 @@
+import board
+import pwmio
+import pio_encoder
+import busio
+import adafruit_vl53l1x
+import math
+import adafruit_bno055
+
+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)
+imu = adafruit_bno055.BNO055_I2C(i2c0)
+
+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())
+
+def check_imu_status():
+  sys_status, gyro, accel, mag = imu.calibration_status
+  send_line(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}")
+  return sys_status == 3

ch-12/4-make-known-turn/code.py

@@ -0,0 +1,59 @@
+import robot
+import pid_controller
+import asyncio
+import time
+
+class IMUTurnController:
+    def __init__(self):
+        self.pid = pid_controller.PIDController(0.01, 0.010, 0)
+        self.target = 0
+
+    def update(self, dt, angle):
+        error = self.target - angle
+        if error > 180:
+            error -= 360
+        elif error < -180:
+            error += 360
+        control_signal = self.pid.calculate(error, dt)
+        robot.set_left(control_signal)
+        robot.set_right(-control_signal)
+
+
+async def command_handler(turn_controller):
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      if command.startswith("-"):
+        turn_controller.target -= int(command.lstrip('-'))
+      elif command.startswith("+"):
+        turn_controller.target += int(command.lstrip('+'))
+      elif command.isdigit():
+        turn_controller.target += int(command)
+    await asyncio.sleep(0)
+
+
+# control loop
+async def control_loop():
+  controller = IMUTurnController()
+  controller.target = robot.imu.euler[0]
+  asyncio.create_task(command_handler(controller))
+  last_time = time.monotonic()
+  while True:
+    await asyncio.sleep(0.1)
+    next_time = time.monotonic()
+    dt = next_time - last_time
+    last_time = next_time
+    angle = robot.imu.euler[0]
+
+    controller.update(dt, angle)
+    robot.send_line(f"{angle}, 0")
+
+
+async def main():
+  while not robot.check_imu_status():
+    await asyncio.sleep(0.1)
+  robot.send_line("Ready to go!")
+
+  await control_loop()
+
+asyncio.run(main())

ch-12/4-make-known-turn/pid_controller.py

@@ -0,0 +1,27 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+        self.imax = imax
+        self.imin = imin
+        self.reset()
+
+    def reset(self):
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+        if self.imax is not None and self.integral > self.imax:
+            self.integral = self.imax
+        if self.imin is not None and self.integral < self.imin:
+            self.integral = self.imin
+        # Add a low pass filter to the difference
+        difference = (error - self.error_prev) * self.d_filter_gain
+        self.error_prev += difference
+        self.derivative = difference / dt
+
+        return self.kp * error + self.ki * self.integral + self.kd * self.derivative

ch-12/4-make-known-turn/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-12/4-make-known-turn/robot.py

@@ -0,0 +1,76 @@
+import board
+import pwmio
+import pio_encoder
+import busio
+import adafruit_vl53l1x
+import math
+import adafruit_bno055
+
+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)
+imu = adafruit_bno055.BNO055_I2C(i2c0)
+
+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())
+
+def check_imu_status():
+  sys_status, gyro, accel, mag = imu.calibration_status
+  send_line(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}")
+  return sys_status == 3