Implemented pointers from Review

fall-2020/navigation.py

@@ -70,24 +70,73 @@ def set_desired_position(self, desired_position):
         Note: assume the user will pass in the desired_position parameter when using
         the interface 
         """
-        pass
+        # This sets the desired position to the value passed by the user when they use the UI
+        self.desired_position = desired_position
+
 
-
     def update_current_position(self):
         """ Updates the current position of the TBM """
-        pass
+        # This updates the current position
+        self.current_position = self.GPS.getPos()
 
 
     def navigate(self):
         """ Navigate to the desired position from the current position
 
         Based on the current position tuple, compared to the desired position tuple,
         make decisions on steering, and ensure that the actuations are successful
-
         Returns: True if actuation requests were successful, False if not
         Note: It may be good to notify the user if something unexpected happens!
+
+        This method was implemented by identifying the difference in the desired position
+        and the current position. Each difference was stored in the variables x, y, and z.
+        Based on these variables, actuations are attempted to move TBM.
+        If all actuations were successful, this method returns True, and the user is notified
+        that everything went well! Otherwise, the user is notified that actuations were 
+        unsuccesful in the corresponding direction (x, y, or z). 
         """
-        pass
+        # Evaluate the differences of each position for x, y, and z
+        x = self.desired_position[0] - self.current_position[0]
+        y = self.desired_position[1] - self.current_position[1]
+        z = self.desired_position[2] - self.current_position[2]
+
+        # First check the x component and attempt actuation
+        if (x > 0):
+            x_act = self.steering.move_forward(x)
+        else:
+            x_act = self.steering.stop()
+
+        # Next check the y component and attempt actuation
+        if (y > 0):
+            y_act = self.steering.move_right(y)
+        elif (y < 0):
+            y_act = self.steering.move_left(y)
+        else:
+            y_act = self.steering.stop()
+
+        # Lastly, check the z component and attempt actuation
+        if (z > 0):
+            z_act = self.steering.move_up(z)
+        elif (z < 0):
+            z_act = self.steering.move_down(z)
+        else:
+            z_act = self.steering.stop()
+
+        # Check if all actuations were successful! Return True if all were successful.
+        if x_act and y_act and z_act:
+            print("All actuations were successful :D")
+            return True
+
+        # Next check all the actuations and print out to the user where TBM is running into errors for either x, y, or z.
+        if not x_act:
+            print("Something went wrong with the actuation moving in the x-direction!")
+        if not y_act:
+            print("Something went wrong with the actuation moving in the y-direction!")
+        if not z_act:
+            print("Something went wrong with the actuation moving in the z-direction!")
+
+        # If not all of the actuations were successful, return False
+        return False
 
 
 # Code below is provided for you, YOU DO NOT NEED TO IMPLEMENT THIS
@@ -108,12 +157,10 @@ def pollSensor(self):
         """
         self.x, self.y, self.z = self.GPS.read()
 
-
     def getPos(self):
         """ Returns the TBM position """
         return self.x, self.y, self.z
 
-
 class Steering:
     """ Interfaces with the actuator which steers the TBM, and controls its speed 
 
@@ -132,7 +179,6 @@ class Steering:
     def __init__(self, actuation_component):
         self.act = actuation_component
 
-
     def move_left(self, left_distance):
         """ Steers the TBM left
 
@@ -154,27 +200,25 @@ 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
 
         Returns: True if the motor stops the TBM, False if it does not
         """
         return self.act.stop()
-
+
+