diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 3cdf661..21e66fb 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -32,27 +32,29 @@ class will interface with our onboard GPS sensor to get thecurrent position, and | | below target - + Misc: TBM - Tunnel Boring Machine """ # Implement this class - this code will not actually be run, it is more of a thought exercise! + + class Navigation: - """ Class to control position based on GPS input - + """ Class to control position based on GPS input + Attributes: GPS - GPS Interface object, used to obtain GPS coordinates steering - Steering object current_position - The current position of the TBM in 3-D space (x, y, z) desired_position - The desired position of the TBM in 3-D space (x, y, z) - + Methods: - set_desired_position() - update_current_position() + set_desired_position() + update_current_position() navigate() """ - + def __init__(self, GPS, steering): # Instances of the two classes defined below for use to complete the navigation method @@ -62,39 +64,102 @@ 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 + 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.x, self.GPS.y, self.GPS.z) 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! """ - pass + + # Calculating individual coordinate distances + x_dist = self.desired_position[0] - self.current_position[0] + y_dist = self.desired_position[1] - self.current_position[1] + z_dist = self.desired_position[2] - self.current_position[2] + + # Checking for the desired actuations + if(x_dist > 0): + if(not self.steering.move_forward(x_dist)): + print( + "Error occured while performing x-direction forward movement!") + return False + + # It is necessary to stop the TBM and update the current position after encountering an error past this point just to make sure the previous actuations are recorded. + # Assuming, the TBM is moving along the positive X-directon, the left side of TBM must be the positive Y-axis and the right must be the negative. + if(y_dist > 0): + if(not self.steering.move_left(y_dist)): + print( + "Error occured while performing y-direction left steering!") + stop_result = self.steering.stop() + # if cannot stop + if(not stop_result): + print("Error occured while stopping the TBM!") + else: + self.update_current_position() + return False + elif(y_dist < 0): + if(not self.steering.move_right(y_dist)): + print( + "Error occured while performing y-direction right steering!") + stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") + else: + self.update_current_position() + return False + if(z_dist > 0): + if(not self.steering.move_up(z_dist)): + print("Error occured while moving up!") + stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") + else: + self.update_current_position() + return False + elif(z_dist < 0): + if(not self.steering.move_down(z_dist)): + print("Error occured while moving down!") + stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") + else: + self.update_current_position() + return False + + # When actuations are complete, stopping the TBM and updating the current position for the next navigation commands. + stop_result = self.steering.stop() + # If the TBM cannot stop, print error and stop it forcefully somehow. + if(not stop_result): + print("Error occured while stopping the TBM!") + return False + else: + self.update_current_position() + print("Navigation complete.") + return True # Code below is provided for you, YOU DO NOT NEED TO IMPLEMENT THIS - + + class GPS: """ Mock GPS sensor interface class """ - + def __init__(self, GPSSensor): self.GPS = GPSSensor self.x = GPSSensor.pos.x @@ -107,16 +172,15 @@ 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 class Steering: - """ Interfaces with the actuator which steers the TBM, and controls its speed - + """ Interfaces with the actuator which steers the TBM, and controls its speed + Attributes: act - Placeholder actuation component (could be a motor for example, or some other actuator related to steering) @@ -128,18 +192,17 @@ class Steering: move_forward() stop() """ - + def __init__(self, actuation_component): self.act = actuation_component - def move_left(self, left_distance): """ Steers the TBM left Returns: True if the actuation is successful, False if it is not """ return self.act.steer_left(left_distance) - + def move_right(self, right_distance): """ Steers the TBM to the right @@ -147,22 +210,20 @@ def move_right(self, right_distance): """ return self.act.steer_right(right_distance) - def move_down(self, down_distance): + def move_down(self, down_distance): """ Steers the TBM down Returns: True if the actuation is successful, False if it is not """ return self.act.steer_down(down_distance) - def move_up(self, up_distance): - """ Steers the TBM to the right + """ Steers the TBM up 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 @@ -170,11 +231,9 @@ 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() -