Ackerman calculations for wheel angles

electrical/breakbeam.py

@@ -6,19 +6,19 @@
 class Breakbeam:
     ACCEPTABLE_TIME = 3  # time to wait to confirm a full break
 
-    class BEAM_PINS(Enum):
-        HALF1 = 17
-        HALF2 = 22
-        FULL1 = 23
-        FULL2 = 24
+    # class BEAM_PINS(Enum):
+    #     HALF1 = 17
+    #     HALF2 = 22
+    #     FULL1 = 23
+    #     FULL2 = 24
 
-    def __init__(self):
+    def __init__(self, pins):
         """
         Initializes each sensor to detect changes. Initializes beam_broken to False.
         Creates an empty list for the beams that are broken.
         Attribute blocked_sensors: set of sensors that have been fully blocked.
         """
-        for BEAM_PIN in Breakbeam.BEAM_PINS:
+        for BEAM_PIN in pins:
             GPIO.setmode(GPIO.BCM)
             GPIO.setup(BEAM_PIN.value, GPIO.IN, pull_up_down=GPIO.PUD_UP)
             GPIO.add_event_detect(
@@ -38,7 +38,7 @@ def break_beam_callback(self, channel):
         # print("50% full: " + str(self.check_half()))
         # print("100% full: " + str(self.check_full()))
 
-    def timer(self, channel):
+    def timer(self, pins, channel):
         """
         Gets the current time and calculates the time that has passed, then compares
         this to the ACCEPTABLE_TIME to determine whether the broken beam was just
@@ -53,32 +53,32 @@ def timer(self, channel):
             if GPIO.input(channel):
                 break
         else:
-            self.blocked_sensors.add(Breakbeam.BEAM_PINS(channel).name)
+            self.blocked_sensors.add(pins(channel).name)
             self.blocked_sensors_check()
             # print(self.blocked_sensors)
 
-    def blocked_sensors_check(self):
+    def blocked_sensors_check(self, pins):
         """
         Checks whether the sensors in the set blocked_sensors are still blocked.
         Uses list TO_BE_REMOVED to store the sensors that have been unblocked and
         then remove them from the set of blocked sensors.
         """
         TO_BE_REMOVED = []  # sensors to remove due to being unbroken
         for sensor in self.blocked_sensors:
-            if GPIO.input(Breakbeam.BEAM_PINS[sensor].value):
+            if GPIO.input(pins[sensor].value):
                 TO_BE_REMOVED.append(sensor)
 
         for sensor in TO_BE_REMOVED:
             self.blocked_sensors.remove(sensor)
 
-    def check_half(self):
+    def check_half(self, pin_half_one, pin_half_two):
         """
         Returns whether the 50% sensors are in the set of fully blocked sensors
         """
         return (
             len(self.blocked_sensors) < 4
-            and Breakbeam.BEAM_PINS(17).name in self.blocked_sensors
-            and Breakbeam.BEAM_PINS(22).name in self.blocked_sensors
+            and pin_half_one in self.blocked_sensors
+            and pin_half_two in self.blocked_sensors
         )
 
     def check_full(self):

engine/robot_logic/traversal.py

@@ -58,59 +58,67 @@ def traverse_standard(robot_state, unvisited_waypoints, allowed_dist_error, data
     unvisited_waypoints.popleft()
 
     return robot_state, unvisited_waypoints
+
+
 """
 Does not use PID, EKF. The method assumes that we are physically running the robot and not running it in simulation.
 TODO: For now, we are keep the original move_to_target_node logic. 
 When we test it on the robot, and if it does not follow the intended trajectory
 then add back Alan's fix.
 """
+
+
 def simple_move_to_target_node(robot_state, target, allowed_dist_error, database):
     """
     Moves robot to target + or - allowed_dist_error
     Arguments:
         target: target coordinates in the form (latitude, longitude)
-        allowed_dist_error: the maximum distance in meters that the robot 
+        allowed_dist_error: the maximum distance in meters that the robot
         can be from a node for the robot to have "visited" that node
-        database: 
+        database:
     """
-    import matplotlib.pyplot as plt 
-
+    import matplotlib.pyplot as plt
 
     # location error (in meters)
     distance_away = calculate_dist(target, robot_state.state)
     robot_state.goal_location = target
-
-    while (distance_away > allowed_dist_error) and not (robot_state.phase == Phase.AVOID_OBSTACLE):
+
+    while (distance_away > allowed_dist_error) and not (
+        robot_state.phase == Phase.AVOID_OBSTACLE
+    ):
         # Error in terms of latitude and longitude, NOT meters
         x_coords_error = target[0] - robot_state.state[0]
         y_coords_error = target[1] - robot_state.state[1]
 
-        desired_angle = math.atan2(y_coords_error,  x_coords_error)
+        desired_angle = math.atan2(y_coords_error, x_coords_error)
 
         x_vel = x_coords_error
         y_vel = y_coords_error
 
         cmd_v, cmd_w = feedback_lin(
-            robot_state.state, x_vel, y_vel, robot_state.epsilon)
+            robot_state.state, x_vel, y_vel, robot_state.epsilon
+        )
 
         # clamping of velocities:
         (limited_cmd_v, limited_cmd_w) = limit_cmds(
-            cmd_v, cmd_w, robot_state.max_velocity, robot_state.radius)
+            cmd_v, cmd_w, robot_state.max_velocity, robot_state.radius
+        )
 
         robot_state.linear_v = limited_cmd_v[0]
         robot_state.angular_v = limited_cmd_w[0]
-        #TODO: we need to unify time.sleep and robot_state.time_step
-        travel(robot_state, robot_state.time_step *
-               limited_cmd_v[0], robot_state.time_step * limited_cmd_w[0])
+        # TODO: we need to unify time.sleep and robot_state.time_step
+        travel(
+            robot_state,
+            robot_state.time_step * limited_cmd_v[0],
+            robot_state.time_step * limited_cmd_w[0],
+        )
         if not robot_state.is_sim:
-            robot_state.motor_controller.spin_motors(
-                limited_cmd_w[0], limited_cmd_v[0])
+            robot_state.motor_controller.spin_motors(limited_cmd_w[0], limited_cmd_v[0])
             time.sleep(10)
 
         # location error (in meters)
         distance_away = calculate_dist(target, robot_state.state)
-        #TODO: Determine whether we want to update database here.
-
+        # TODO: Determine whether we want to update database here.
 
 
 def move_to_target_node(robot_state, target, allowed_dist_error, database):
@@ -145,7 +153,7 @@ def move_to_target_node(robot_state, target, allowed_dist_error, database):
             iterations += 1
             if iterations == 50:
                 break
-        desired_angle = math.atan2( y_coords_error,  x_coords_error)
+        desired_angle = math.atan2(y_coords_error, x_coords_error)
 
         x_vel = robot_state.loc_pid_x.update(x_coords_error)
         y_vel = robot_state.loc_pid_y.update(y_coords_error)
@@ -392,4 +400,85 @@ def turn_to_target_heading(
         abs_heading_error = abs(target_heading - float(predicted_state[2]))
     # re-enable after finishing turning
     robot_state.enable_obstacle_avoidance = True
-    travel(robot_state, 0, 0)
+    if not robot_state.is_sim:
+        robot_state.motor_controller.spin_motors(0, 0)
+
+    # Ackerman calculations calculates the ideal wheel angles, and then uses the
+    # ackerman percentage to adjust the outside wheel angle
+
+
+def ackerman_calculations(left_ang, right_ang, inside_ang, wheel_base, steering_ratio):
+    # calculate ideal wheel angle
+    ackerman_percentage = 0
+
+    # calculate desired width of turn
+    track_width = wheel_base * ((1 / np.tan(left_ang) - (1 / np.tan(right_ang))))
+
+    # ackerman ratio used in adjusting wheel angles
+    ackerman_ratio = inside_ang / steering_ratio
+
+    # apply the ackerman ratio to our wheels
+    new_left_ang = 1 / np.tan(
+        (wheel_base * np.tan(ackerman_ratio))
+        / (wheel_base + (0.5 * track_width)(np.tan(ackerman_ratio)))
+    )
+    new_right_ang = 1 / np.tan(
+        (wheel_base * np.tan(ackerman_ratio))
+        / (wheel_base - (0.5 * track_width)(np.tan(ackerman_ratio)))
+    )
+    new_inside = new_left_ang
+    outside_ang = new_inside - ackerman_percentage * (new_inside - ackerman_ratio)
+
+    # return needed data to complete turns
+    return [
+        track_width,
+        ackerman_ratio,
+        new_left_ang,
+        new_right_ang,
+        new_inside,
+        outside_ang,
+    ]
+
+
+def ackerman_right(left_ang, right_ang, inside_ang, wheel_base, steering_ratio):
+    """
+    Calculates the angle needed for the vehicle to turn right based on Ackermann steering geometry.
+
+    Arguments:
+        left_ang (float): The angle of the left wheel in radians.
+        right_ang (float): The angle of the right wheel in radians.
+        inside_ang (float): The angle of the inside wheel in radians.
+        wheel_base (float): The distance between the centers of the front and rear axles of the vehicle.
+        steering_ratio (float): The ratio between the angle of the steering wheel and the angle of the wheels.
+
+    Returns:
+        float: The angle needed for the vehicle to turn right, in radians.
+    """
+    result = ackerman_calculations(
+        left_ang, right_ang, inside_ang, wheel_base, steering_ratio
+    )
+    # The angle needed to turn is the difference between the new right angle and the original inside angle
+    turn_angle = (result[4] - inside_ang) % (2 * math.pi)
+    return turn_angle
+
+
+def ackerman_left(left_ang, right_ang, inside_ang, wheel_base, steering_ratio):
+    """
+    Calculates the angle needed for the vehicle to turn left based on Ackermann steering geometry.
+
+    Arguments:
+        left_ang (float): The angle of the left wheel in radians.
+        right_ang (float): The angle of the right wheel in radians.
+        inside_ang (float): The angle of the inside wheel in radians.
+        wheel_base (float): The distance between the centers of the front and rear axles of the vehicle.
+        steering_ratio (float): The ratio between the angle of the steering wheel and the angle of the wheels.
+
+    Returns:
+        float: The angle needed for the vehicle to turn left, in radians.
+    """
+    result = ackerman_calculations(
+        left_ang, right_ang, inside_ang, wheel_base, steering_ratio
+    )
+    # The angle needed to turn is the difference between the new left angle and the original inside angle
+    turn_angle = (result[4] - inside_ang) % (2 * math.pi)
+    return turn_angle

tests/functionality_tests/traversal_test.py

@@ -0,0 +1,39 @@
+import math
+import unittest
+from engine.robot_logic.traversal import (
+    ackerman_calculations,
+    ackerman_right,
+    ackerman_left,
+)
+
+
+class TestAckermanFunctions(unittest.TestCase):
+    def test_ackerman_right(self):
+        # Test case where the robot needs to turn right
+        left_ang = math.radians(30)  # in radians
+        right_ang = math.radians(45)  # in radians
+        inside_ang = math.radians(30)  # in radians
+        wheel_base = 1.5  # in meters
+        steering_ratio = 1  # arbitrary value
+        expected_turn_angle = math.radians(45 - inside_ang)
+        actual_turn_angle = math.radians(
+            ackerman_right(left_ang, right_ang, inside_ang, wheel_base, steering_ratio)
+        )
+        self.assertEqual(expected_turn_angle, actual_turn_angle)
+
+    def test_ackerman_left(self):
+        # Test case where the robot needs to turn left
+        left_ang = math.radians(45)  # in radians
+        right_ang = math.radians(30)  # in radians
+        inside_ang = math.radians(45)  # in radians
+        wheel_base = 1.5  # in meters
+        steering_ratio = 1  # arbitrary value
+        expected_turn_angle = math.radians(45 - inside_ang)
+        actual_turn_angle = math.radians(
+            ackerman_left(left_ang, right_ang, inside_ang, wheel_base, steering_ratio)
+        )
+        self.assertEqual(expected_turn_angle, actual_turn_angle)
+
+
+if __name__ == "__main__":
+    unittest.main()