diff --git a/electrical/breakbeam.py b/electrical/breakbeam.py index 6cc9959b..fd26941e 100644 --- a/electrical/breakbeam.py +++ b/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,11 +53,11 @@ 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 @@ -65,20 +65,20 @@ def blocked_sensors_check(self): """ 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): diff --git a/engine/robot_logic/traversal.py b/engine/robot_logic/traversal.py index 626ab776..030f5342 100644 --- a/engine/robot_logic/traversal.py +++ b/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) \ No newline at end of file + 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 diff --git a/tests/functionality_tests/traversal_test.py b/tests/functionality_tests/traversal_test.py new file mode 100644 index 00000000..7199bdfb --- /dev/null +++ b/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()