Chapter 8 code

ch-8/1-tests/read_1_sensor.py

@@ -0,0 +1,19 @@
+import time
+import board
+import busio
+import adafruit_vl53l1x
+
+# i2c = busio.I2C(sda=board.GP0, scl=board.GP1)
+i2c = busio.I2C(sda=board.GP2, scl=board.GP3)
+vl53 = adafruit_vl53l1x.VL53L1X(i2c)
+
+vl53.distance_mode = 1
+
+vl53.timing_budget = 100
+vl53.start_ranging()
+
+while True:
+    if vl53.data_ready:
+        print("Distance: {} cm".format(vl53.distance))
+        vl53.clear_interrupt()
+    time.sleep(0.05)

ch-8/2-two-sensors/read_2_sensors.py

@@ -0,0 +1,30 @@
+import time
+import board
+import busio
+import adafruit_vl53l1x
+
+i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
+i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
+
+## If you accidentally pass the same bus twice, you'll get the same measurements
+vl53_l = adafruit_vl53l1x.VL53L1X(i2c0)
+vl53_r = adafruit_vl53l1x.VL53L1X(i2c1)
+
+
+# we want it short range
+vl53_l.distance_mode = 1
+vl53_l.timing_budget = 100
+
+vl53_r.distance_mode = 1
+vl53_r.timing_budget = 100
+
+vl53_l.start_ranging()
+vl53_r.start_ranging()
+
+
+while True:
+    if vl53_l.data_ready and vl53_r.data_ready:
+        print("Left: {} cm, Right: {} cm".format(vl53_l.distance, vl53_r.distance))
+        vl53_l.clear_interrupt()
+        vl53_r.clear_interrupt()
+    time.sleep(0.05)

ch-8/3-avoid/avoid_walls.py

@@ -0,0 +1,43 @@
+import robot
+import time
+
+robot.left_distance.distance_mode = 1
+robot.right_distance.distance_mode = 1
+
+too_close_cm = 30
+speed = 0.9
+
+robot.left_distance.start_ranging()
+robot.right_distance.start_ranging()
+
+try:
+  robot.set_left(speed)
+  robot.set_right(speed)
+  
+  while True:
+    # do we have data:
+    if robot.left_distance.data_ready and robot.right_distance.data_ready:
+      left_dist = robot.left_distance.distance
+      right_dist = robot.right_distance.distance
+      # are any too close
+      if right_dist < too_close_cm:
+        print("Obstacle detected - Left: {} cm, Right: {} cm".format(left_dist, right_dist))
+        robot.set_left(-speed)
+      else:
+        robot.set_left(speed)
+        if left_dist < too_close_cm:
+          print("Obstacle detected - Left: {} cm, Right: {} cm".format(left_dist, right_dist))
+          robot.set_right(-speed)
+        else:
+          robot.set_right(speed)
+
+      robot.left_distance.clear_interrupt()
+      robot.right_distance.clear_interrupt()
+      time.sleep(0.1)
+
+finally:
+  robot.stop()
+  robot.left_distance.clear_interrupt()
+  robot.right_distance.clear_interrupt()
+  robot.left_distance.stop_ranging()
+  robot.right_distance.stop_ranging()

ch-8/3-avoid/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-8/3-avoid/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)
+
+left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
+right_distance = adafruit_vl53l1x.VL53L1X(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 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-8/3-avoid/varying_wall_avoid.py

@@ -0,0 +1,40 @@
+
+import robot
+import time
+
+robot.left_distance.distance_mode = 1
+robot.left_distance.start_ranging()
+robot.right_distance.distance_mode = 1
+robot.right_distance.start_ranging()
+
+speed = 0.9
+
+def speed_from_distance(distance):
+  limited_distance = min(distance, 30) * speed
+  return limited_distance / 30
+
+try:
+  robot.set_left(speed)
+  robot.set_right(speed)
+  
+  while True:
+    # do we have data:
+    if robot.left_distance.data_ready and robot.right_distance.data_ready:
+      left_dist = robot.left_distance.distance
+      right_dist = robot.right_distance.distance
+      l_speed = speed_from_distance(robot.right_distance.distance)
+      r_speed = speed_from_distance(robot.left_distance.distance)
+      print("Left: {} cm R Sp: {}, Right: {} cm L Sp: {}".format(left_dist, r_speed, right_dist, l_speed))
+      robot.set_left(l_speed)
+      robot.set_right(r_speed)
+
+      robot.left_distance.clear_interrupt()
+      robot.right_distance.clear_interrupt()
+      time.sleep(0.1)
+
+finally:
+  robot.stop()
+  robot.left_distance.clear_interrupt()
+  robot.right_distance.clear_interrupt()
+  robot.left_distance.stop_ranging()
+  robot.right_distance.stop_ranging()