Improve passive collision avoidance and motion

Co-authored-by: Jason Brown <[email protected]>

Author: WeixuanZ

driver/driver.py

@@ -4,6 +4,9 @@
 """This is the driver script of the controller
 """
 from robot import IDPRobot
+from modules.utils import print_if_debug
+
+DEBUG_OBJECTIVE = False
 
 robot = IDPRobot()
 complete = False
@@ -12,7 +15,7 @@
 while robot.step(robot.timestep) != -1:
 
     # Check if we are finished
-    if robot.target_cache.num_collected >= 4 or robot.targeting_handler.num_scans >= 10:
+    if robot.target_cache.num_collected >= 4 or robot.targeting_handler.num_scans >= 15:
         if robot.distance_from_bot(robot.home) <= 0.1:
             robot.do("hold")
             if not complete:
@@ -24,10 +27,14 @@
     # If we have no action
     if robot.execute_next_action():
         # If we have a target go to it, else scan
-        if robot.get_best_target():
+        if target := robot.get_best_target():
+            print_if_debug(f"{robot.color}, objective: Collecting block at {target.position}",
+                           debug_flag=DEBUG_OBJECTIVE)
             robot.do("collect")
         else:
             other_bot_collected = robot.radio.get_other_bot_collected()
             if other_bot_collected is not None and len(other_bot_collected) == 4:
                 robot.target_cache.update_flipped(robot.color)
+            print_if_debug(f"{robot.color}, objective: No target, scanning",
+                           debug_flag=DEBUG_OBJECTIVE)
             robot.do("scan")

driver/modules/motion.py

@@ -15,11 +15,8 @@ class MotionCS:
     To be used via robot.motors.velocities = MotionControlStrategies.some_method(*args)
     """
 
-    # Some characteristics of the robot used for motion calcs
-    max_f_speed = 1.0
-    max_r_speed = 11.2
-    f_drive_speed_ratio = 1 / max_f_speed
-    r_drive_speed_ratio = 1 / max_r_speed
+    # Overwritten in robot.py
+    max_f_speed = None
 
     @staticmethod
     def combine_and_scale(forward, rotation, angle=None):
@@ -103,8 +100,8 @@ def linear_dual_pid(distance=None, forward_speed=None, distance_pid=None, forwar
             req_forward_speed *= attenuation_factor
 
         # Convert from actual robot velocities to drive fraction equivalent
-        required_forward_drive = MotionCS.f_drive_speed_ratio * req_forward_speed
-        forward_speed_drive_eq = None if forward_speed is None else MotionCS.f_drive_speed_ratio * forward_speed
+        required_forward_drive = req_forward_speed / MotionCS.max_f_speed
+        forward_speed_drive_eq = None if forward_speed is None else forward_speed / MotionCS.max_f_speed
 
         return MotionCS.dual_pid(prime_quantity=distance, deriv_quantity=forward_speed_drive_eq,
                                  prime_pid=distance_pid, derivative_pid=forward_speed_pid,

driver/modules/pid.py

@@ -173,8 +173,11 @@ def plot_history(self, *args):
 {f" d_weight_decay_half_life={self.d_weight_decay_half_life} " if self.d_weight_decay_half_life is not None else ""}"""
 
         if not args:
-            plot_args = ["output", "error"] + (["p"] * bool(self.k_p)) + (["cumulative_error", "i"] * bool(self.k_i))\
-                + (["error_change", "d"] * bool(self.k_d))  # Now this is pod-racing
+            if self.custom_function is None:
+                plot_args = ["output", "error"] + (["p"] * bool(self.k_p)) + (["cumulative_error", "i"] * bool(self.k_i))\
+                    + (["error_change", "d"] * bool(self.k_d))  # Now this is pod-racing
+            else:
+                plot_args = ["output", "error", "cumulative_error", "error_change"]
         else:
             plot_args = args
 

driver/pid_tuning.py

@@ -1,7 +1,6 @@
 # Test script for drive_to_pos
 
 import numpy as np
-from modules.utils import fire_and_forget
 
 tau = np.pi * 2
 
@@ -13,8 +12,7 @@ def manual(robot):
 
     # Actions for our robot
     action_queue = [
-        ("hold", 2),
-        ("move", [0.5, 0])
+        ("move", [-0.5, -0.4])
     ]
 
     robot.action_queue = action_queue
@@ -23,7 +21,4 @@ def manual(robot):
     while robot.step(timestep) != -1:
         robot.execute_next_action()
 
-    fire_and_forget(robot.plot_motion_history)
-    fire_and_forget(robot.pid_f_velocity.plot_history)
-    fire_and_forget(robot.pid_distance.plot_history)
-    fire_and_forget(robot.pid_angle.plot_history)
+    robot.plot_all_graphs()

driver/robot.py

@@ -0,0 +1,22 @@
+# Test script for drive_to_pos
+
+def main(robot):
+    # Setup
+    timestep = int(robot.getBasicTimeStep())
+
+    # Actions for our robot
+    action_queue = [
+        ("move", [0, 0]),
+        "scan",
+        ("move", [0.9, 0.9]),
+        ("move", [0.9, -0.9]),
+        ("move", [-0.9, 0.9]),
+        ("move", [-0.9, -0.9]),
+        ("move", [0, 0])
+    ]
+
+    robot.action_queue = action_queue
+
+    # Main loop, perform simulation steps until Webots is stopping the controller
+    while robot.step(timestep) != -1:
+        robot.execute_next_action()

driver/tests/collision_avoidance.py

@@ -12,9 +12,8 @@ def main(robot):
     # Main loop, perform simulation steps until Webots is stopping the controller
     while robot.step(timestep) != -1:
 
-        robot.motors.velocities = np.array([0.5, -0.5])
+        robot.motors.velocities = np.array([1, -1])
         robot.update_motion_history(time=robot.time, linear_speed=robot.linear_speed,
                                     angular_velocity=robot.angular_velocity)
-        print(robot.linear_speed)
 
     robot.plot_motion_history()

driver/tests/manual_test.py

@@ -23,3 +23,5 @@ def main(robot):
     # Main loop, perform simulation steps until Webots is stopping the controller
     while robot.step(timestep) != -1:
         robot.execute_next_action()
+
+    robot.plot_all_graphs()

driver/tests/motion_test.py

@@ -18,11 +18,12 @@
 from pid_tuning import manual as pid_tuning
 from tests.manual_test import main as manual_test
 from tests.color_test import main as color_test
+from tests.collision_avoidance import main as collision_avoidance_test
 
 if __name__ == '__main__':
     robot = IDPRobot()
     # com_test(robot)
-    # motion_test(robot)
+    motion_test(robot)
     # camera_test(robot)
     # gate_test(robot)
     # object_processing_test(robot)
@@ -31,6 +32,7 @@
     # sensor_bounds_ultrasonic_test(robot)
     # sensor_bounds_ir_test(robot)
     # greedy_collect(robot)
-    pid_tuning(robot)
+    # pid_tuning(robot)
     # manual_test(robot)
     # color_test(robot)
+    collision_avoidance_test(robot)