diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 3cdf661..ec2fe04 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -52,7 +52,6 @@ class Navigation: update_current_position() navigate() """ - def __init__(self, GPS, steering): # Instances of the two classes defined below for use to complete the navigation method @@ -62,7 +61,6 @@ def __init__(self, GPS, steering): # These will be tuples of the form (x, y, z) self.current_position = None self.desired_position = None - def set_desired_position(self, desired_position): """ Sets the desired position the TBM will attempt to move to. @@ -70,13 +68,12 @@ def set_desired_position(self, desired_position): Note: assume the user will pass in the desired_position parameter when using the interface """ - pass + self.desired_position = desired_position - def update_current_position(self): """ Updates the current position of the TBM """ - pass - + self.GPS.pollsensor() + self.current_position = self.GPS.getPos() def navigate(self): """ Navigate to the desired position from the current position @@ -87,11 +84,82 @@ def navigate(self): Returns: True if actuation requests were successful, False if not Note: It may be good to notify the user if something unexpected happens! """ - pass + # Getting current position of the TBM. + self.update_current_position() + + if self.current_position == self.desired_position: + print("Current position is already at desired position.") + return True + + # Looping through and comparing each corresponding coordinate in the two position tuples. + for coordinate_index in range(3): + # If x => check if TBM is behind, if y => check if TBM is left, if z => check if TBM is below. + if self.current_position[coordinate_index] < self.desired_position[coordinate_index]: + # Distance between the two corresponding x, y, or z values. + steer_distance = self.desired_position[coordinate_index] - self.current_position[coordinate_index] + + # x value. + if coordinate_index == 0: + if self.steering.move_forward(): + print("Actuation successful.") + else: + print("Forward actuation unsuccessful.") + + # y value. + elif coordinate_index == 1: + if self.steering.move_right(steer_distance): + print("Actuation successful.") + else: + print("Right actuation unsuccessful.") + + # z value. + elif coordinate_index == 2: + if self.steering.move_up(steer_distance): + print("Actuation successful.") + else: + print("Up actuation unsuccessful.") + + # If x => check if TBM is in front, if y => check if TBM is right, if z => check if TBM is above. + elif self.current_position[coordinate_index] > self.desired_position[coordinate_index]: + steer_distance = self.current_position[coordinate_index] - self.desired_position[coordinate_index] + + if coordinate_index == 0: + raise ValueError("Unexpected input for x coordinate. " + "Should be behind target not in front of target.") + + elif coordinate_index == 1: + if self.steering.move_left(steer_distance): + print("Actuation successful.") + else: + print("Left actuation unsuccessful.") + + elif coordinate_index == 2: + if self.steering.move_down(steer_distance): + print("Actuation successful.") + else: + print("Down actuation unsuccessful.") + + # Getting new current position of the TBM. + self.update_current_position() + + if self.current_position == self.desired_position: + if self.steering.stop(): + print("Stop successful. All actuation's successful.") + return True + else: + print("All actuation's successful but failed to stop.") + return False + + else: + if self.steering.stop(): + print("Actuation's unsuccessful desired destination not reached.") + return False + else: + print("Actuation's unsuccessful and failed to stop.") + return False # Code below is provided for you, YOU DO NOT NEED TO IMPLEMENT THIS - class GPS: """ Mock GPS sensor interface class """ @@ -107,8 +175,7 @@ def pollSensor(self): Updates the current position by reading the gps sensor """ self.x, self.y, self.z = self.GPS.read() - - + def getPos(self): """ Returns the TBM position """ return self.x, self.y, self.z @@ -132,7 +199,6 @@ class Steering: def __init__(self, actuation_component): self.act = actuation_component - def move_left(self, left_distance): """ Steers the TBM left @@ -154,7 +220,6 @@ def move_down(self, down_distance): """ return self.act.steer_down(down_distance) - def move_up(self, up_distance): """ Steers the TBM to the right @@ -162,7 +227,6 @@ def move_up(self, up_distance): """ return self.act.steer_up(up_distance) - def move_forward(self): """ Moves the TBM forward @@ -170,7 +234,6 @@ def move_forward(self): """ return self.act.forward() - def stop(self): """ Stops the TBM from moving