Wall following demo

ch-10/1-follow/follow.py

@@ -1,45 +1,54 @@
-import robot
 import time
+import robot
 import pid
 
-robot.left_distance.distance_mode = 1
+class FollowObject:
+  def __init__(self):
+    self.max_speed = 0.9
+    self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15)
+    self.last_time = time.monotonic_ns()
+    self.left_dist = 0
+    self.pid_output = 0
 
-max_speed = 0.9
-set_point = 15
+  def setup_robot(self):
+    robot.left_distance.distance_mode = 1
 
-robot.left_distance.start_ranging()
-
-follow_pid = pid.PID(0.1, 0, 0)
-last_time = time.monotonic()
-print("Starting")
-try:
-  while True:
+  def movement_update(self):
     # 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
+      self.left_dist = robot.left_distance.distance
       
       # calculate time delta
-      new_time = time.monotonic()
-      time_delta = new_time - last_time
-      last_time = new_time
+      new_time = time.monotonic_ns()
+      time_delta = new_time - self.last_time
+      self.last_time = new_time
 
       # get speeds from pid
-      speed = min(max_speed, follow_pid.update(error_value, time_delta))
-      speed = max(-max_speed, speed)
+      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
-      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()
+  def main_loop(self):
+    robot.left_distance.start_ranging()
+    self.last_time = time.monotonic()
+    while True:
+      self.movement_update()
+
+  def start(self):
+    print("Starting")
+    try:
+      self.setup_robot()
+      self.main_loop()
+    finally:
+      robot.stop()
+      robot.left_distance.clear_interrupt()
+      robot.left_distance.stop_ranging()
+
+FollowObject().start()

ch-10/1-follow/pid.py

@@ -1,17 +1,21 @@
 class PID:
-  def __init__(self, proportional_k, integral_k, differential_k) -> None:
+  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.integral = 0
+    self.error_sum = 0
     self.last_value = 0
 
-  def update(self, error_value, time_delta):
+  def update(self, measurement, time_delta):
+    error_value = measurement - self.set_point
     proportional = error_value * self.proportional_k
 
-    self.integral += error_value * time_delta
-    integral = self.integral * self.integral_k
+    # calculate integral
+    self.error_sum += error_value * time_delta
+    integral = self.error_sum * self.integral_k
+    self.last_value = error_value
 
     differentiated_error = (error_value - self.last_value) / time_delta
     differential = differentiated_error * self.differential_k