From e4d3c2c459642889260bcb0d7d9254bc5ab456d9 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:18:57 -0330 Subject: [PATCH 01/11] Update navigation.py --- fall-2020/navigation.py | 89 +++++++++++++++++++++++++++-------------- 1 file changed, 60 insertions(+), 29 deletions(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 3cdf661..4a1db9a 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,74 @@ 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 """ + self.desired_position = desired_position pass - def update_current_position(self): """ Updates the current position of the TBM """ + self.GPS.pollSensor(self.GPS) + self.current_position = (self.GPS.x, self.GPS.y, self.GPS.z) pass - 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 + # distance_left = ((self.desired_position[0]-self.current_position[0])**2 + + # (self.desired_position[1]-self.current_position[1])**2 + + # (self.desired_position[2]-self.current_position[2])**2)**(1/2) + # + + 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.desired_position[2] + + if(x_dist > 0): + if(not self.steering.move_forward(x_dist)): + print( + "Error occured while performing x-direction forward movement!") + return False + if(y_dist > 0): + if(not self.steering.move_left(y_dist)): + print( + "Error occured while performing y-direction left steering!") + return False + elif(y_dist < 0): + if(not self.steering.move_right(y_dist)): + print( + "Error occured while performing y-direction right steering!") + return False + if(z_dist > 0): + if(not self.steering.move_up(z_dist)): + print("Error occured while moving up!") + return False + elif(z_dist < 0): + if(not self.steering.move_down(z_dist)): + print("Error occured while moving down!") + return False + + self.steering.stop() + self.update_current_position() # 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 +144,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 +164,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 +182,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 +203,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() - From 18b7b716b930835cbb9590014fa8641a467fe03c Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:22:52 -0330 Subject: [PATCH 02/11] Update navigation.py --- fall-2020/navigation.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 4a1db9a..5ea9cb5 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -90,15 +90,12 @@ def navigate(self): Note: It may be good to notify the user if something unexpected happens! """ - # distance_left = ((self.desired_position[0]-self.current_position[0])**2 + - # (self.desired_position[1]-self.current_position[1])**2 + - # (self.desired_position[2]-self.current_position[2])**2)**(1/2) - # - + #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.desired_position[2] + #Checking for the desired actuations if(x_dist > 0): if(not self.steering.move_forward(x_dist)): print( @@ -123,6 +120,7 @@ def navigate(self): print("Error occured while moving down!") return False + #When actuations are complete, stopping the TBM and updating the current position for the next navigation commands. self.steering.stop() self.update_current_position() From 6c3c1f2dbcbbc2d193e14924461e2bd154f49690 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:27:13 -0330 Subject: [PATCH 03/11] Update navigation.py --- fall-2020/navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 5ea9cb5..a69a511 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -76,7 +76,7 @@ def set_desired_position(self, desired_position): def update_current_position(self): """ Updates the current position of the TBM """ - self.GPS.pollSensor(self.GPS) + self.GPS.pollSensor() self.current_position = (self.GPS.x, self.GPS.y, self.GPS.z) pass From c6ad01673eccaa4c751777d706a97e8216b3a86e Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:36:11 -0330 Subject: [PATCH 04/11] Update navigation.py --- fall-2020/navigation.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index a69a511..5b1b491 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -72,13 +72,11 @@ def set_desired_position(self, desired_position): the interface """ self.desired_position = desired_position - pass def update_current_position(self): """ Updates the current position of the TBM """ self.GPS.pollSensor() self.current_position = (self.GPS.x, self.GPS.y, self.GPS.z) - pass def navigate(self): """ Navigate to the desired position from the current position @@ -101,23 +99,29 @@ def navigate(self): print( "Error occured while performing x-direction forward movement!") return False + + #It is necessary to update the current position after encountering an error past this point just to make sure the previous actuations are recorded. if(y_dist > 0): if(not self.steering.move_left(y_dist)): print( "Error occured while performing y-direction left steering!") + 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!") + self.update_current_position() return False if(z_dist > 0): if(not self.steering.move_up(z_dist)): print("Error occured while moving up!") + self.update_current_position() return False elif(z_dist < 0): if(not self.steering.move_down(z_dist)): print("Error occured while moving down!") + self.update_current_position() return False #When actuations are complete, stopping the TBM and updating the current position for the next navigation commands. From fa65694bc011dde152fa2399a0861272415f4964 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:38:39 -0330 Subject: [PATCH 05/11] Update navigation.py --- fall-2020/navigation.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 5b1b491..3d92935 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -88,43 +88,47 @@ def navigate(self): Note: It may be good to notify the user if something unexpected happens! """ - #Calculating individual coordinate distances + # 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.desired_position[2] - #Checking for the desired actuations + # 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 update the current position after encountering an error past this point just to make sure the previous actuations are recorded. + # 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. if(y_dist > 0): if(not self.steering.move_left(y_dist)): + self.steering.stop() print( "Error occured while performing y-direction left steering!") - self.update_current_position() + self.update_current_position() return False elif(y_dist < 0): if(not self.steering.move_right(y_dist)): + self.steering.stop() print( "Error occured while performing y-direction right steering!") - self.update_current_position() + self.update_current_position() return False if(z_dist > 0): if(not self.steering.move_up(z_dist)): + self.steering.stop() print("Error occured while moving up!") self.update_current_position() return False elif(z_dist < 0): if(not self.steering.move_down(z_dist)): + self.steering.stop() print("Error occured while moving down!") self.update_current_position() return False - #When actuations are complete, stopping the TBM and updating the current position for the next navigation commands. + # When actuations are complete, stopping the TBM and updating the current position for the next navigation commands. self.steering.stop() self.update_current_position() From 0b24eb3bb045a5c7cbeef217f8b83e8f9149de55 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:42:21 -0330 Subject: [PATCH 06/11] Update navigation.py --- fall-2020/navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 3d92935..7ad4a71 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -91,7 +91,7 @@ def navigate(self): # 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.desired_position[2] + z_dist = self.desired_position[2] - self.current_position[2] # Checking for the desired actuations if(x_dist > 0): From 98fd2dec24e1d261ddedb818f4c3e30ef8699822 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:45:36 -0330 Subject: [PATCH 07/11] Update navigation.py --- fall-2020/navigation.py | 1 + 1 file changed, 1 insertion(+) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 7ad4a71..79860eb 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -101,6 +101,7 @@ def navigate(self): 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 on positive X-directon, the left side of TBM must be positive Y-axis and the right must be the negative. if(y_dist > 0): if(not self.steering.move_left(y_dist)): self.steering.stop() From 780bd451be1c9210b2d2b372c6b14894ce4915a7 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:46:25 -0330 Subject: [PATCH 08/11] Update navigation.py --- fall-2020/navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 79860eb..b5d73a3 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -101,7 +101,7 @@ def navigate(self): 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 on positive X-directon, the left side of TBM must be positive Y-axis and the right must be the negative. + # 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)): self.steering.stop() From 3f2a2b271c99849fc6f08ba82ecedbd860914a77 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:53:46 -0330 Subject: [PATCH 09/11] Update navigation.py --- fall-2020/navigation.py | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index b5d73a3..01473b9 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -104,34 +104,42 @@ def navigate(self): # 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)): - self.steering.stop() + stop_result = self.steering.stop() print( "Error occured while performing y-direction left steering!") self.update_current_position() return False elif(y_dist < 0): if(not self.steering.move_right(y_dist)): - self.steering.stop() + stop_result = self.steering.stop() print( "Error occured while performing y-direction right steering!") self.update_current_position() return False if(z_dist > 0): if(not self.steering.move_up(z_dist)): - self.steering.stop() + stop_result = self.steering.stop() print("Error occured while moving up!") self.update_current_position() return False elif(z_dist < 0): if(not self.steering.move_down(z_dist)): - self.steering.stop() + stop_result = self.steering.stop() print("Error occured while moving down!") self.update_current_position() return False # When actuations are complete, stopping the TBM and updating the current position for the next navigation commands. - self.steering.stop() - self.update_current_position() + 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 From 66d8eefa075390653f265f23af40363bbc3e0e10 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 11:57:39 -0330 Subject: [PATCH 10/11] Update navigation.py --- fall-2020/navigation.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index 01473b9..d1cfa79 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -105,6 +105,8 @@ def navigate(self): if(y_dist > 0): if(not self.steering.move_left(y_dist)): stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") print( "Error occured while performing y-direction left steering!") self.update_current_position() @@ -112,6 +114,8 @@ def navigate(self): elif(y_dist < 0): if(not self.steering.move_right(y_dist)): stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") print( "Error occured while performing y-direction right steering!") self.update_current_position() @@ -119,12 +123,16 @@ def navigate(self): if(z_dist > 0): if(not self.steering.move_up(z_dist)): stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") print("Error occured while moving up!") self.update_current_position() return False elif(z_dist < 0): if(not self.steering.move_down(z_dist)): stop_result = self.steering.stop() + if(not stop_result): + print("Error occured while stopping the TBM!") print("Error occured while moving down!") self.update_current_position() return False From 3da3017218bebb324f0c162fe6f9dbf913e67112 Mon Sep 17 00:00:00 2001 From: Anuj Sevak Date: Tue, 3 Nov 2020 12:01:56 -0330 Subject: [PATCH 11/11] Update navigation.py --- fall-2020/navigation.py | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/fall-2020/navigation.py b/fall-2020/navigation.py index d1cfa79..21e66fb 100644 --- a/fall-2020/navigation.py +++ b/fall-2020/navigation.py @@ -104,37 +104,42 @@ def navigate(self): # 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!") - print( - "Error occured while performing y-direction left steering!") - self.update_current_position() + 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!") - print( - "Error occured while performing y-direction right steering!") - self.update_current_position() + 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!") - print("Error occured while moving up!") - self.update_current_position() + 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!") - print("Error occured while moving down!") - self.update_current_position() + else: + self.update_current_position() return False # When actuations are complete, stopping the TBM and updating the current position for the next navigation commands.