Wall following demo

ch-10/2-graphing/graphing_follow.py

@@ -4,15 +4,15 @@ import json
 from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
 from adafruit_wsgi.wsgi_app import WSGIApp
 
-import pid
 import robot
 import robot_wifi
+import pid
 
 
-class FollowApp:
-  def __init__(self) -> None:    
+class FollowObject:
+  def __init__(self):
     self.max_speed = 0.9
-    self.follow_pid = pid.PID(0.1, 0.1, 0.015)
+    self.follow_pid = pid.PID(0.1, 0.1, 0.015, 15)
     self.wifi = None
     self.server = None
 
@@ -38,10 +38,9 @@ class FollowApp:
     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,
+        "last_value": self.follow_pid.last_value,
         "pid_output": self.pid_output,
         "time": self.last_time
       }
@@ -59,12 +58,9 @@ class FollowApp:
 
       # 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)
+      speed = self.pid_output * self.max_speed
 
       # make movements
-      if abs(speed) < 0.3:
-        speed = 0
       robot.set_left(speed)
       robot.set_right(speed)
 
@@ -98,4 +94,4 @@ class FollowApp:
       robot.left_distance.clear_interrupt()
       robot.left_distance.stop_ranging()
 
-FollowApp().start()
+FollowObject().start()

ch-10/2-graphing/pc/live_graph.py

@@ -17,7 +17,11 @@ class SensorStream:
       self.times = []
 
   def sensor_stream(self, frame):
-    response = requests.get(url, timeout=1)
+    try:
+      response = requests.get(url, timeout=1)
+    except (requests.exceptions.ConnectTimeout, requests.exceptions.ReadTimeout, requests.exceptions.ConnectionError):
+      print("Waiting...")
+      return
     print(f"Content: {response.content}")
     print(f"status: {response.status_code}")
 

ch-10/2-graphing/pid.py

@@ -1,3 +1,6 @@
+from black import err
+
+
 class PID:
   def __init__(self, proportional_k, integral_k, differential_k, set_point):
     self.proportional_k = proportional_k    
@@ -10,12 +13,13 @@ class PID:
     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   
+    # calculate integral
     self.error_sum += error_value * time_delta
     # clamp it
     self.error_sum = min(self.max_output, self.error_sum)
@@ -25,10 +29,14 @@ class PID:
 
     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
-    output = min(self.max_output, output)
-    output = max(self.min_output, output)
+    if abs(output) < self.dead_zone:
+      output = 0
+    else:
+      output = min(self.max_output, output)
+      output = max(self.min_output, output)
 
     return output