diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 3cdf661..c7b6512 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -70,12 +70,14 @@ 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): @@ -83,11 +85,58 @@ def navigate(self): 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,7 +200,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 +207,6 @@ def move_up(self, up_distance): """ return self.act.steer_up(up_distance) - def move_forward(self): """ Moves the TBM forward @@ -170,11 +214,11 @@ def move_forward(self): """ 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() - + + \ No newline at end of file