My code implementation for software challenge

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,21 +61,19 @@ 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.
 
         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,23 +220,20 @@ 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
 
         Returns: True if the actuation is successful, False if it is not
         """
         return self.act.steer_up(up_distance)
 
-
     def move_forward(self):
         """ Moves the TBM forward
 
         Returns: True if the motor sucessfully moves the TBM, False if it is not
         """
         return self.act.forward()
 
-
     def stop(self):
         """ Stops the TBM from moving