Wall following demo

ch-10/3-wall-follow/wall_follow.py

@@ -10,28 +10,24 @@ import robot
 import robot_wifi
 
 
-app = WSGIApp()
-
 class FollowWallApp:
   def __init__(self) -> None:    
-    self.    self.speed = 0.6
- = 0.6
+    self.speed = 0.6
     self.max_deflection = 0.4
+    self.follow_pid = pid.PID(0.1, 0.5, 0, 15)
+    self.follow_pid.dead_zone = 0.6
 
-    self.set_point = 15
-    self.follow_pid = pid.PID(0.1, 0, 0)
     self.wifi = None
     self.server = None
 
-    self.last_time = 0
+    self.last_time = time.monotonic_ns()
     self.left_dist = 0
-    self.error_value = 0
     self.pid_output = 0
 
-  def setup_robot():
+  def setup_robot(self):
     robot.left_distance.distance_mode = 1
 
-  def setup_wifi(self):
+  def setup_wifi(self, app):
     print("Setting up wifi.")
     self.wifi, esp = robot_wifi.connect_to_wifi()
     self.server = adafruit_esp32spi_wsgiserver.WSGIServer(
@@ -45,43 +41,32 @@ class FollowWallApp:
     ip_int = ".".join(str(int(n)) for n in esp.ip_address)
     print(f"IP Address is {ip_int}")
 
-  @app.route("/")
   def index(self, request):
     return 200, [('Content-Type', 'application/json')], [json.dumps(
       {
-        "error_value": self.error_value,
-        "left_dist": self.left_dist,
+        "last_value": self.follow_pid.last_value,
         "pid_output": self.pid_output,
-        "last_time": self.last_time
+        "time": self.last_time
       }
-    )]      
-
-  @app.route("/set_point")
-  def set_point(self, request):
-    return 200, [('Content-Type', 'application/json')], [json.dumps(self.set_point)]
+    )]
 
   def movement_update(self):
     # do we have data
     if robot.left_distance.data_ready:
       self.left_dist = robot.left_distance.distance
       
-      # get error value
-      self.error_value = self.left_dist - self.set_point
-
       # calculate time delta
-      new_time = time.monotonic()
+      new_time = time.monotonic_ns()
       time_delta = new_time - self.last_time
       self.last_time = new_time
 
       # get turn from pid
-      self.pid_output = self.follow_pid.update(self.error_value, time_delta)
-      deflection = min(self.max_deflection, self.pid_output)
-      deflection = max(-self.max_deflection, deflection)
+      self.pid_output = self.follow_pid.update(self.left_dist, time_delta)
+      deflection = self.pid_output * self.max_deflection
 
       # make movements
-      print(f"Dist: {self.left_dist}, Err: {self.error_value}, Deflection: {deflection}")
-      robot.set_left(self.speed + deflection)
-      robot.set_right(self.speed - deflection)
+      robot.set_left(self.speed - deflection)
+      robot.set_right(self.speed + deflection)
 
       # reset and loop
       robot.left_distance.clear_interrupt()
@@ -93,7 +78,6 @@ class FollowWallApp:
       try:
         self.movement_update()
         self.server.update_poll()
-        time.sleep(0.1)
       except RuntimeError as e:
         print(f"Server poll error: {type(e)}, {e}")
         robot.stop()
@@ -102,10 +86,12 @@ class FollowWallApp:
         print("Reset complete.")
 
   def start(self):
+    app = WSGIApp()
+    app.route("/")(self.index)
     print("Starting")
     try:
       self.setup_robot()
-      self.setup_wifi()
+      self.setup_wifi(app)
       self.main_loop()
     finally:
       robot.stop()