From 72f23a0fffbfd4e63f367f5a4d1248fbe0b715a1 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 15:49:12 +0200 Subject: [PATCH 01/21] Update original.py --- Eurobot_2024/original.py | 60 ++++++++++++++++++++++++++++++++++------ 1 file changed, 51 insertions(+), 9 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index ec3359c..bc1d610 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -28,7 +28,7 @@ def __init__(self): #MyRio(program='4a'), Point(x=-1.0, y=0.0), Angle(angle=90.0), - Point(x=-1.0, y=5.0), + Point(x=-1.0, y=5.0), Angle(angle=135.0), #Point(x=0.0, y=5.0), MyRio(program='4a'), @@ -64,6 +64,7 @@ def lidar_callback(self, msg): min_distance = min(msg.ranges) if min_distance < self.min_distance: self.stop_robot() + #do a 45 turn to avoid. def odom_callback(self, msg): self.robot_x = msg.pose.pose.position.x @@ -81,8 +82,35 @@ def odom_callback(self, msg): target = self.path[self.current_path_index] print("TARGET: ", self.current_path_index) - #print("coordenadas: :", [self.robot_x, self.robot_y]) - if isinstance(target, Point): + + # if isinstance(target, Point): + # distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + # angle_to_target = (math.atan2(self.robot_x - target.x, self.robot_y - target.y) * 180) / math.pi + + # if abs(angle_to_target) > 90: + # direction = -1 + # else: + # direction = 1 + + # #giro + # if 0.3 < angle_to_target < -0.3: #el rango es menos al "recto" ya que el if está arriba, deja un margen por si nos desviamos al ir recto + # rotate_to_angle(angle_to_target) #piendo que self.angle es en grados y va de -180 a 180 + + # if -0.5 < angle_to_target < 0.5 and distance_to_target > self.distance_threshold: + # #hay que implemetar una función que detecte si la distacia aumenta, para parar + # self.move_towards_point(target) + + # else: + # self.current_path_index += 1 + # if self.current_path_index < len(self.path): + # self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + # else: + # self.get_logger().info("Reached the end of the path.") + # self.stop_robot() + + + + if isinstance(target, Point) distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): @@ -130,14 +158,28 @@ def odom_callback(self, msg): def move_towards_point(self, target_point): twist_msg = Twist() angle_to_target_radians = math.atan2(target_point.y - self.robot_y, target_point.x - self.robot_x) - angle_to_target = np.degrees(angle_to_target_radians) - target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) + angle_to_target = np.degrees(angle_to_target_radians) - self.current_angle + + #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) print("target vector: ", target_vector) - if -0.15 < target_vector[0] < 0.15 or -0.15 < target_vector[1] < 0.15: - twist_msg.linear.x = -0.5 + + if abs(angle_to_target) < 90: + new_angle_to_target = angle_to_target + direction = 1 + + else: #para ir marcha atras + new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 + direction = -1 + + #girar + if abs(new_angle_to_target - self.current_angle) > 0.5: #si el desvio es mayor a 0.5º, girar + twist_msg.angular.z = 0.005 * (new_angle_to_target - self.current_angle) #completar las partes ID del PID + self.cmd_vel_pub.publish(twist_msg) + #avanzar else: - twist_msg.linear.x = 0.5 # Forward velocity - self.cmd_vel_pub.publish(twist_msg) + twist_msg.linear.x = 0.5 * direction + self.cmd_vel_pub.publish(twist_msg) + def rotate_to_angle(self, target_angle): twist_msg = Twist() From ee5b448baf2969c7b56ed5568e25602d91c6b689 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 17:08:33 +0200 Subject: [PATCH 02/21] Update original.py --- Eurobot_2024/original.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index bc1d610..0283cb3 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -58,6 +58,8 @@ def __init__(self): self.robot_quaternion_z = None self.robot_quaternion_w = None + self.prev_dist = 99999999999 + def lidar_callback(self, msg): time.sleep(0.08) @@ -110,11 +112,11 @@ def odom_callback(self, msg): - if isinstance(target, Point) + if isinstance(target, Point): distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if distance_to_target > self.distance_threshold: + if (prev_dist - distance_to_target) > 0: self.move_towards_point(target) else: self.current_path_index += 1 @@ -123,7 +125,7 @@ def odom_callback(self, msg): else: self.get_logger().info("Reached the end of the path.") self.stop_robot() - #prev_dist = distance_to_target + prev_dist = distance_to_target elif isinstance(target, Angle): @@ -161,24 +163,24 @@ def move_towards_point(self, target_point): angle_to_target = np.degrees(angle_to_target_radians) - self.current_angle #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) - print("target vector: ", target_vector) + #print("target vector: ", target_vector) - if abs(angle_to_target) < 90: + if abs(angle_to_target) <= 90: new_angle_to_target = angle_to_target direction = 1 - else: #para ir marcha atras + elif abs(angle_to_target) > 90: #para ir marcha atras new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 direction = -1 #girar - if abs(new_angle_to_target - self.current_angle) > 0.5: #si el desvio es mayor a 0.5º, girar + elif abs(new_angle_to_target - self.current_angle) > 1: #si el desvio es mayor a 0.5º, girar twist_msg.angular.z = 0.005 * (new_angle_to_target - self.current_angle) #completar las partes ID del PID - self.cmd_vel_pub.publish(twist_msg) #avanzar - else: + elif abs(new_angle_to_target - self.current_angle) < 1: twist_msg.linear.x = 0.5 * direction - self.cmd_vel_pub.publish(twist_msg) + + self.cmd_vel_pub.publish(twist_msg) def rotate_to_angle(self, target_angle): From b98ba86048ca8fbea5b81faa09bfe6ade8008c37 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 17:11:59 +0200 Subject: [PATCH 03/21] V2 --- Eurobot_2024/original.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 0283cb3..3325f8d 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -174,8 +174,8 @@ def move_towards_point(self, target_point): direction = -1 #girar - elif abs(new_angle_to_target - self.current_angle) > 1: #si el desvio es mayor a 0.5º, girar - twist_msg.angular.z = 0.005 * (new_angle_to_target - self.current_angle) #completar las partes ID del PID + elif abs(new_angle_to_target - self.current_angle) > 0.5: #si el desvio es mayor a 0.5º, girar + twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID #avanzar elif abs(new_angle_to_target - self.current_angle) < 1: twist_msg.linear.x = 0.5 * direction From 32f363072ca312acaeef2fdc1a421dac087e169c Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 17:44:34 +0200 Subject: [PATCH 04/21] V2 --- Eurobot_2024/original.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 3325f8d..0283cb3 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -174,8 +174,8 @@ def move_towards_point(self, target_point): direction = -1 #girar - elif abs(new_angle_to_target - self.current_angle) > 0.5: #si el desvio es mayor a 0.5º, girar - twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID + elif abs(new_angle_to_target - self.current_angle) > 1: #si el desvio es mayor a 0.5º, girar + twist_msg.angular.z = 0.005 * (new_angle_to_target - self.current_angle) #completar las partes ID del PID #avanzar elif abs(new_angle_to_target - self.current_angle) < 1: twist_msg.linear.x = 0.5 * direction From 907d8a202d7fea1c0ac534f9141c37b641b247ad Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 17:54:04 +0200 Subject: [PATCH 05/21] V2.1 --- Eurobot_2024/original.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 0283cb3..1f47bf0 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -174,10 +174,10 @@ def move_towards_point(self, target_point): direction = -1 #girar - elif abs(new_angle_to_target - self.current_angle) > 1: #si el desvio es mayor a 0.5º, girar - twist_msg.angular.z = 0.005 * (new_angle_to_target - self.current_angle) #completar las partes ID del PID + if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar + twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID #avanzar - elif abs(new_angle_to_target - self.current_angle) < 1: + elif abs(new_angle_to_target) < 1: twist_msg.linear.x = 0.5 * direction self.cmd_vel_pub.publish(twist_msg) From 601e912236b61b1e8a42cc3047c0b0fa65639204 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 18:10:43 +0200 Subject: [PATCH 06/21] V2.2 --- Eurobot_2024/original.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 1f47bf0..c09d3c3 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -116,7 +116,7 @@ def odom_callback(self, msg): distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if (prev_dist - distance_to_target) > 0: + if (self.prev_dist - distance_to_target) > 0: self.move_towards_point(target) else: self.current_path_index += 1 @@ -125,7 +125,7 @@ def odom_callback(self, msg): else: self.get_logger().info("Reached the end of the path.") self.stop_robot() - prev_dist = distance_to_target + self.prev_dist = distance_to_target elif isinstance(target, Angle): @@ -163,7 +163,7 @@ def move_towards_point(self, target_point): angle_to_target = np.degrees(angle_to_target_radians) - self.current_angle #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) - #print("target vector: ", target_vector) + print("target rotation: ", angle_to_target) if abs(angle_to_target) <= 90: new_angle_to_target = angle_to_target @@ -176,9 +176,11 @@ def move_towards_point(self, target_point): #girar if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID + print("girando") #avanzar - elif abs(new_angle_to_target) < 1: + elif abs(new_angle_to_target) <= 1: twist_msg.linear.x = 0.5 * direction + print("avanzando") self.cmd_vel_pub.publish(twist_msg) From 2d7a9e80aaee9cb5e472b78a1f435e1839921e2e Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 18:13:23 +0200 Subject: [PATCH 07/21] V2.2 --- Eurobot_2024/original.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index c09d3c3..100c041 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -168,10 +168,12 @@ def move_towards_point(self, target_point): if abs(angle_to_target) <= 90: new_angle_to_target = angle_to_target direction = 1 + print("direction: 1") elif abs(angle_to_target) > 90: #para ir marcha atras new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 direction = -1 + print("direction: -1") #girar if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar From 067c9fc132ef484ee4222fa0e7b4ad64000d3efa Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 18:23:33 +0200 Subject: [PATCH 08/21] 2.3 --- Eurobot_2024/original.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 100c041..6ca9cd2 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -168,21 +168,22 @@ def move_towards_point(self, target_point): if abs(angle_to_target) <= 90: new_angle_to_target = angle_to_target direction = 1 - print("direction: 1") + print("direction: 1") elif abs(angle_to_target) > 90: #para ir marcha atras new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 direction = -1 - print("direction: -1") + print("direction: -1") + print("new target rotation: ", new_angle_to_target) #girar if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID - print("girando") + print("girando") #avanzar elif abs(new_angle_to_target) <= 1: twist_msg.linear.x = 0.5 * direction - print("avanzando") + print("avanzando") self.cmd_vel_pub.publish(twist_msg) From 36279709ef3a087b4cc302a0f981eb20fc7c1cbe Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 18:34:23 +0200 Subject: [PATCH 09/21] 2.3 --- Eurobot_2024/original.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 6ca9cd2..5ec920d 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -27,19 +27,19 @@ def __init__(self): self.path = [ #MyRio(program='4a'), Point(x=-1.0, y=0.0), - Angle(angle=90.0), + #Angle(angle=90.0), Point(x=-1.0, y=5.0), - Angle(angle=135.0), + #Angle(angle=135.0), #Point(x=0.0, y=5.0), MyRio(program='4a'), Point(x=-1.0, y=5.0), - Angle(angle=90.0), + #Angle(angle=90.0), Point(x=-1.0, y=10.0), - Angle(angle=0.0), + #Angle(angle=0.0), Point(x=0.0, y=10.0), MyRio(program='4a'), Point(x=-1.0, y=10.0), - Angle(angle=-90.0), + #Angle(angle=-90.0), Point(x=-1.0, y=0.0), ] From babb254e7e068f2c7af6a10a290b9e67b1a04ee8 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 18:35:31 +0200 Subject: [PATCH 10/21] V2.3 --- Eurobot_2024/original.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 5ec920d..a2449ec 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -116,7 +116,7 @@ def odom_callback(self, msg): distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if (self.prev_dist - distance_to_target) > 0: + if (self.prev_dist - distance_to_target) >= 0: self.move_towards_point(target) else: self.current_path_index += 1 From 48c6ac9b8718098cd8c2a5c7b057475304b99126 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 18:50:56 +0200 Subject: [PATCH 11/21] V2.4 --- Eurobot_2024/original.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index a2449ec..04838e1 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -116,7 +116,7 @@ def odom_callback(self, msg): distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if (self.prev_dist - distance_to_target) >= 0: + if distance_to_target < self.distance_threshold: self.move_towards_point(target) else: self.current_path_index += 1 From a96eea742c7bb447d69ed35400d47349a7f91df0 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 19:08:00 +0200 Subject: [PATCH 12/21] V2.4.1 --- Eurobot_2024/original.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 04838e1..57395ef 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -27,19 +27,19 @@ def __init__(self): self.path = [ #MyRio(program='4a'), Point(x=-1.0, y=0.0), - #Angle(angle=90.0), + Angle(angle=90.0), Point(x=-1.0, y=5.0), - #Angle(angle=135.0), + Angle(angle=135.0), #Point(x=0.0, y=5.0), MyRio(program='4a'), Point(x=-1.0, y=5.0), - #Angle(angle=90.0), + Angle(angle=90.0), Point(x=-1.0, y=10.0), - #Angle(angle=0.0), + Angle(angle=0.0), Point(x=0.0, y=10.0), MyRio(program='4a'), Point(x=-1.0, y=10.0), - #Angle(angle=-90.0), + Angle(angle=-90.0), Point(x=-1.0, y=0.0), ] @@ -116,7 +116,7 @@ def odom_callback(self, msg): distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if distance_to_target < self.distance_threshold: + if (self.prev_dist - distance_to_target) > 0: self.move_towards_point(target) else: self.current_path_index += 1 @@ -168,22 +168,22 @@ def move_towards_point(self, target_point): if abs(angle_to_target) <= 90: new_angle_to_target = angle_to_target direction = 1 - print("direction: 1") + print("direction: 1") elif abs(angle_to_target) > 90: #para ir marcha atras new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 direction = -1 - print("direction: -1") + print("direction: -1") print("new target rotation: ", new_angle_to_target) #girar if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID - print("girando") + print("girando") #avanzar elif abs(new_angle_to_target) <= 1: twist_msg.linear.x = 0.5 * direction - print("avanzando") + print("avanzando") self.cmd_vel_pub.publish(twist_msg) From 74ee749c9f2e899bc178746d9c511bc9344fc02e Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 19:50:42 +0200 Subject: [PATCH 13/21] V2.4.2 --- Eurobot_2024/original.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 57395ef..70e7ce2 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -26,12 +26,12 @@ def __init__(self): self.path = [ #MyRio(program='4a'), - Point(x=-1.0, y=0.0), - Angle(angle=90.0), - Point(x=-1.0, y=5.0), - Angle(angle=135.0), + Point(x=-10.0, y=0.0), + #Angle(angle=90.0), + Point(x=-5.0, y=-5.0), + #Angle(angle=135.0), #Point(x=0.0, y=5.0), - MyRio(program='4a'), + #MyRio(program='4a'), Point(x=-1.0, y=5.0), Angle(angle=90.0), Point(x=-1.0, y=10.0), @@ -118,6 +118,7 @@ def odom_callback(self, msg): #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): if (self.prev_dist - distance_to_target) > 0: self.move_towards_point(target) + print("target distance: ", self.prev_dist) else: self.current_path_index += 1 if self.current_path_index < len(self.path): @@ -180,12 +181,14 @@ def move_towards_point(self, target_point): if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID print("girando") + self.cmd_vel_pub.publish(twist_msg) #avanzar elif abs(new_angle_to_target) <= 1: twist_msg.linear.x = 0.5 * direction print("avanzando") + self.cmd_vel_pub.publish(twist_msg) + - self.cmd_vel_pub.publish(twist_msg) def rotate_to_angle(self, target_angle): From 7a4286a27b562c12c73f7598a21ecfcb12e68a9d Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 20:03:42 +0200 Subject: [PATCH 14/21] V2.5 --- Eurobot_2024/original.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 70e7ce2..567cec1 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -79,8 +79,8 @@ def odom_callback(self, msg): self.robot_quaternion_w = msg.pose.pose.orientation.w orientation_list = [msg.pose.pose.orientation.x ,msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) - self.current_angle = yaw - print("ANGLE: ", math.degrees(self.current_angle)) + self.current_angle = math.degrees(yaw) + print("ANGLE: ", self.current_angle) target = self.path[self.current_path_index] print("TARGET: ", self.current_path_index) From d813ab989b0c168dbe5b7367fb2286443319079e Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 20:08:42 +0200 Subject: [PATCH 15/21] V2.6 --- Eurobot_2024/original.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 567cec1..931e26c 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -179,7 +179,7 @@ def move_towards_point(self, target_point): print("new target rotation: ", new_angle_to_target) #girar if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar - twist_msg.angular.z = 0.005 * (new_angle_to_target) #completar las partes ID del PID + twist_msg.angular.z = 0.17 * (new_angle_to_target / abs(new_angle_to_target)) print("girando") self.cmd_vel_pub.publish(twist_msg) #avanzar From c9ebaa27e3966698a4472bf8a98c30628a9e72df Mon Sep 17 00:00:00 2001 From: sfty-bit <80603778+sfty-bit@users.noreply.github.com> Date: Tue, 9 Apr 2024 21:11:24 +0200 Subject: [PATCH 16/21] VF_1 --- Eurobot_2024/original.py | 33 +++++++++++++++++++-------------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index 931e26c..bc8f30c 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -26,13 +26,15 @@ def __init__(self): self.path = [ #MyRio(program='4a'), - Point(x=-10.0, y=0.0), + Point(x=-1.0, y=0.0), #Angle(angle=90.0), - Point(x=-5.0, y=-5.0), + Point(x=-1.0, y=5.0), #Angle(angle=135.0), #Point(x=0.0, y=5.0), #MyRio(program='4a'), - Point(x=-1.0, y=5.0), + Point(x=-6.0, y=5.0), + Point(x=-6.0, y=0.0), + Point(x=-1.0, y=0.0), Angle(angle=90.0), Point(x=-1.0, y=10.0), Angle(angle=0.0), @@ -116,17 +118,19 @@ def odom_callback(self, msg): distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if (self.prev_dist - distance_to_target) > 0: + if distance_to_target > 0.1: self.move_towards_point(target) - print("target distance: ", self.prev_dist) + print("target distance: ", self.prev_dist) + self.prev_dist = distance_to_target else: self.current_path_index += 1 + self.prev_dist = 999999999 if self.current_path_index < len(self.path): self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") else: self.get_logger().info("Reached the end of the path.") self.stop_robot() - self.prev_dist = distance_to_target + elif isinstance(target, Angle): @@ -166,27 +170,28 @@ def move_towards_point(self, target_point): #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) print("target rotation: ", angle_to_target) - if abs(angle_to_target) <= 90: + if abs(angle_to_target) <= 100: new_angle_to_target = angle_to_target direction = 1 print("direction: 1") - elif abs(angle_to_target) > 90: #para ir marcha atras + elif abs(angle_to_target) > 100: #para ir marcha atras new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 direction = -1 print("direction: -1") print("new target rotation: ", new_angle_to_target) #girar - if abs(new_angle_to_target) > 1: #si el desvio es mayor a 0.5º, girar - twist_msg.angular.z = 0.17 * (new_angle_to_target / abs(new_angle_to_target)) - print("girando") - self.cmd_vel_pub.publish(twist_msg) + if abs(new_angle_to_target) > 2: #si el desvio es mayor a 2º, girar + while abs(new_angle_to_target) > 1: #margen de correcion + twist_msg.angular.z = 0.17 * (new_angle_to_target / abs(new_angle_to_target)) #falta por implementar PID + print("girando") + self.cmd_vel_pub.publish(twist_msg) #avanzar elif abs(new_angle_to_target) <= 1: - twist_msg.linear.x = 0.5 * direction + twist_msg.linear.x = 0.75 * direction print("avanzando") - self.cmd_vel_pub.publish(twist_msg) + self.cmd_vel_pub.publish(twist_msg) From aff0f4092424e92e786367051835f8084149f901 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 9 Apr 2024 21:15:14 +0200 Subject: [PATCH 17/21] VF_1 From fec4ddf1a066bd00c84e586b142c4f7b83b77e6f Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Wed, 10 Apr 2024 16:10:25 +0200 Subject: [PATCH 18/21] VF1.1 --- Eurobot_2024/original.py | 236 +++++++++++++++++++++------------------ 1 file changed, 130 insertions(+), 106 deletions(-) diff --git a/Eurobot_2024/original.py b/Eurobot_2024/original.py index bc8f30c..4ef0c25 100644 --- a/Eurobot_2024/original.py +++ b/Eurobot_2024/original.py @@ -11,6 +11,9 @@ from custom_msgs.msg import Angle, MyRio # Import the custom Angle message from tf_transformations import euler_from_quaternion +wp.wiringpiPiSetupGpio() +start = 26 +side = 13 class RobotController(Node): def __init__(self): @@ -23,28 +26,51 @@ def __init__(self): self.lidar_callback, 10 ) - - self.path = [ - #MyRio(program='4a'), - Point(x=-1.0, y=0.0), - #Angle(angle=90.0), - Point(x=-1.0, y=5.0), - #Angle(angle=135.0), - #Point(x=0.0, y=5.0), - #MyRio(program='4a'), - Point(x=-6.0, y=5.0), - Point(x=-6.0, y=0.0), - Point(x=-1.0, y=0.0), - Angle(angle=90.0), - Point(x=-1.0, y=10.0), - Angle(angle=0.0), - Point(x=0.0, y=10.0), - MyRio(program='4a'), - Point(x=-1.0, y=10.0), - Angle(angle=-90.0), - Point(x=-1.0, y=0.0), - - ] + #if side == HIGH or LOW para determinar el lado de la pista + if wp.digitalRead(side) == wp.LOW: + self.path = [ + #MyRio(program='4a'), + Point(x=-1.0, y=0.0), + #Angle(angle=90.0), + Point(x=-1.0, y=5.0), + #Angle(angle=135.0), + #Point(x=0.0, y=5.0), + #MyRio(program='4a'), + Point(x=-6.0, y=5.0), + Point(x=-6.0, y=0.0), + Point(x=-1.0, y=0.0), + Angle(angle=90.0), + Point(x=-1.0, y=10.0), + Angle(angle=0.0), + Point(x=0.0, y=10.0), + MyRio(program='4a'), + Point(x=-1.0, y=10.0), + Angle(angle=-90.0), + Point(x=-1.0, y=0.0), + + ] + elif wp.digitalRead(side) == wp.HIGH: + self.path = [ + #MyRio(program='4a'), + Point(x=-2.0, y=0.0), + #Angle(angle=90.0), + Point(x=-2.0, y=-5.0), + #Angle(angle=135.0), + #Point(x=0.0, y=-5.0), + #MyRio(program='4a'), + Point(x=-7.0, y=-5.0), + Point(x=-7.0, y=0.0), + Point(x=-2.0, y=0.0), + Angle(angle=90.0), + Point(x=-1.0, y=10.0), + Angle(angle=0.0), + Point(x=0.0, y=10.0), + MyRio(program='4a'), + Point(x=-1.0, y=10.0), + Angle(angle=-90.0), + Point(x=-1.0, y=0.0), + + ] self.min_distance = 0.5 @@ -60,9 +86,6 @@ def __init__(self): self.robot_quaternion_z = None self.robot_quaternion_w = None - self.prev_dist = 99999999999 - - def lidar_callback(self, msg): time.sleep(0.08) min_distance = min(msg.ranges) @@ -71,96 +94,97 @@ def lidar_callback(self, msg): #do a 45 turn to avoid. def odom_callback(self, msg): - self.robot_x = msg.pose.pose.position.x - self.robot_y = msg.pose.pose.position.y - position = (self.robot_x, self.robot_y) - print("POSICION: ", position) - self.robot_quaternion_x = msg.pose.pose.orientation.x - self.robot_quaternion_y = msg.pose.pose.orientation.y - self.robot_quaternion_z = msg.pose.pose.orientation.z - self.robot_quaternion_w = msg.pose.pose.orientation.w - orientation_list = [msg.pose.pose.orientation.x ,msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] - (roll, pitch, yaw) = euler_from_quaternion(orientation_list) - self.current_angle = math.degrees(yaw) - print("ANGLE: ", self.current_angle) - target = self.path[self.current_path_index] - print("TARGET: ", self.current_path_index) - + if wp.digitalRead(start) == wp.LOW: + self.robot_x = msg.pose.pose.position.x + self.robot_y = msg.pose.pose.position.y + position = (self.robot_x, self.robot_y) + #print("POSICION: ", position) + self.robot_quaternion_x = msg.pose.pose.orientation.x + self.robot_quaternion_y = msg.pose.pose.orientation.y + self.robot_quaternion_z = msg.pose.pose.orientation.z + self.robot_quaternion_w = msg.pose.pose.orientation.w + orientation_list = [msg.pose.pose.orientation.x ,msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) + self.current_angle = math.degrees(yaw) + print("ANGLE: ", self.current_angle) + target = self.path[self.current_path_index] + print("TARGET: ", self.current_path_index) + - # if isinstance(target, Point): - # distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) - # angle_to_target = (math.atan2(self.robot_x - target.x, self.robot_y - target.y) * 180) / math.pi - - # if abs(angle_to_target) > 90: - # direction = -1 - # else: - # direction = 1 - - # #giro - # if 0.3 < angle_to_target < -0.3: #el rango es menos al "recto" ya que el if está arriba, deja un margen por si nos desviamos al ir recto - # rotate_to_angle(angle_to_target) #piendo que self.angle es en grados y va de -180 a 180 - - # if -0.5 < angle_to_target < 0.5 and distance_to_target > self.distance_threshold: - # #hay que implemetar una función que detecte si la distacia aumenta, para parar - # self.move_towards_point(target) - - # else: - # self.current_path_index += 1 - # if self.current_path_index < len(self.path): - # self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") - # else: - # self.get_logger().info("Reached the end of the path.") - # self.stop_robot() - - - - if isinstance(target, Point): - distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) - #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) - #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): - if distance_to_target > 0.1: - self.move_towards_point(target) - print("target distance: ", self.prev_dist) - self.prev_dist = distance_to_target - else: - self.current_path_index += 1 - self.prev_dist = 999999999 - if self.current_path_index < len(self.path): - self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + # if isinstance(target, Point): + # distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + # angle_to_target = (math.atan2(self.robot_x - target.x, self.robot_y - target.y) * 180) / math.pi + + # if abs(angle_to_target) > 90: + # direction = -1 + # else: + # direction = 1 + + # #giro + # if 0.3 < angle_to_target < -0.3: #el rango es menos al "recto" ya que el if está arriba, deja un margen por si nos desviamos al ir recto + # rotate_to_angle(angle_to_target) #piendo que self.angle es en grados y va de -180 a 180 + + # if -0.5 < angle_to_target < 0.5 and distance_to_target > self.distance_threshold: + # #hay que implemetar una función que detecte si la distacia aumenta, para parar + # self.move_towards_point(target) + + # else: + # self.current_path_index += 1 + # if self.current_path_index < len(self.path): + # self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + # else: + # self.get_logger().info("Reached the end of the path.") + # self.stop_robot() + + + + if isinstance(target, Point): + distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) + #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): + if distance_to_target > 0.1: + self.move_towards_point(target) + print("target distance: ", self.prev_dist) + self.prev_dist = distance_to_target else: - self.get_logger().info("Reached the end of the path.") - self.stop_robot() - + self.current_path_index += 1 - elif isinstance(target, Angle): + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + + elif isinstance(target, Angle): - angle_to_target = ((target.angle * math.pi/180.0) - self.current_angle) - if angle_to_target > self.distance_threshold: - self.rotate_to_angle(target) - else: - self.current_path_index += 1 - if self.current_path_index < len(self.path): - self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + angle_to_target = ((target.angle * math.pi/180.0) - self.current_angle) + if angle_to_target > self.distance_threshold: + self.rotate_to_angle(target) else: - self.get_logger().info("Reached the end of the path.") - self.stop_robot() + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + elif isinstance(target, MyRio): + baud = 9600 + program = target.program + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, program) + char_val = wp.serialGetchar(serial) - elif isinstance(target, MyRio): - baud = 9600 - program = target.program - serial = wp.serialOpen("/dev/serial0", baud) - wp.serialPuts(serial, program) - char_val = wp.serialGetchar(serial) + + if char_val == 102: + self.current_path_index += 1 + self.get_logger().info(f"SECUENCIA BRAZO TERMINADA {self.current_path_index}. Moving to next point...") - - if char_val == 102: - self.current_path_index += 1 - self.get_logger().info(f"SECUENCIA BRAZO TERMINADA {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") - else: - self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") - def move_towards_point(self, target_point): twist_msg = Twist() From 0527dd4e4a6780c3520e5eef2489b513c1529354 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 30 Apr 2024 19:15:11 +0200 Subject: [PATCH 19/21] Create Path_with_simple_PID_NO_tunned --- Eurobot_2024/Path_with_simple_PID_NO_tunned | 359 ++++++++++++++++++++ 1 file changed, 359 insertions(+) create mode 100644 Eurobot_2024/Path_with_simple_PID_NO_tunned diff --git a/Eurobot_2024/Path_with_simple_PID_NO_tunned b/Eurobot_2024/Path_with_simple_PID_NO_tunned new file mode 100644 index 0000000..8c9eb82 --- /dev/null +++ b/Eurobot_2024/Path_with_simple_PID_NO_tunned @@ -0,0 +1,359 @@ +#BASIC PATH +import math +import time +import numpy as np +import wiringpi as wp +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, Point +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +from custom_msgs.msg import Angle, MyRio, SIMA # Import the custom Angle message +from tf_transformations import euler_from_quaternion + +wp.wiringPiSetupGpio() +start = 26 +side = 13 + +class RobotController(Node): + def __init__(self): + super().__init__('robot_controller') + self.cmd_vel_pub = self.create_publisher(Twist, 'diff_cont/cmd_vel_unstamped', 10) + self.odom_sub = self.create_subscription(Odometry, 'diff_cont/odom', self.odom_callback, 10) + self.subscription = self.create_subscription( + LaserScan, + '/scan', # Use '/scan' as the topic name + self.lidar_callback, + 10 + ) + if wp.digitalRead(side) == wp.LOW: #Lado amarillo + ########### el 3 es amarillo + ########### el 4 es azul + self.path = [ + SIMA(code = 'b'), + MyRio(program='4a'), + Point(x=-1.0, y=0.0), + Point(x=-1.0, y=-2.3), + Angle(angle = 0.0), + Point(x=0.0, y=-2.3), + MyRio(program='4a'), + Point(x=-1.0, y=-2.3), + Point(x=-1.0, y=-4.4), + Angle(angle = 0.0), + Point(x=0.0, y=-4.4), + MyRio(program='4a'), + Point(x=-1.0, y=-4.4), + #Point(x=-1.0, y=-10.0), + Point(x=-9.0, y=-13.0), + Angle(angle=135.0), + Point(x=-14.0, y=0.0), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=0.0), + # #Angle(angle=90.0), + # Point(x=-1.0, y=2.25), + # Angle(angle=0.0),ç + + # Point(x=0.0, y=2.25), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=2.25), + ########### el 3 es amarillo + # Point(x=-1.0, y=4.5), + # Angle(angle=0.0), + # Point(x=0.0, y=4.5), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=4.5), + # Point(x=-1.0, y=10.0), + # Angle(angle=0.0), + # Point(x=0.0, y=10.0), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-5.0, y=25.0), + # Point(x=-15.0, y=0.5), + # Angle(angle=135.0), + # Point(x=0.0, y=5.0), + # MyRio(program='4a'), + # Point(x=-6.0, y=5.0), + # Point(x=-6.0, y=0.0), + # Point(x=-1.0, y=0.0), + # Angle(angle=90.0), + # Point(x=-1.0, y=10.0), + # Angle(angle=0.0), + # Point(x=0.0, y=10.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=10.0), + # Angle(angle=-90.0), + # Point(x=-1.0, y=0.0), + ] + if wp.digitalRead(side) == wp.HIGH: #Lado azul + self.path = [ + Point(x=2.0, y=0.0), + Point(x=2.0, y=10.0), + Point(x=10.0, y=10.0), + Point(x=10.0, y=-10.0), + Point(x=2.0, y=-10.0), + Point(x=2.0, y=0.0), + Angle(0.0), + Point(x=0.0, y=0.0), + # #MyRio(program='4a'), + # Point(x=-1.0, y=0.0), + # #Angle(angle=90.0), + # Point(x=-1.0, y=-5.0), + # #Angle(angle=135.0), + # #Point(x=0.0, y=5.0), + # #MyRio(program='4a'), + # Point(x=-6.0, y=-5.0), + # Point(x=-6.0, y=0.0), + # Point(x=-1.0, y=0.0), + # Angle(angle=90.0), + # Point(x=-1.0, y=10.0), + # Angle(angle=0.0), + # Point(x=0.0, y=10.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=10.0), + # Angle(angle=-90.0), + # Point(x=-1.0, y=0.0), + ] + + + + self.min_distance = 0.5 + self.COLLISION = False + + self.current_path_index = 0 + self.distance_threshold = 0.1 # Set your distance threshold for stopping + self.robot_x = None + self.robot_y = None + + self.send_once = False + + self.robot_angle = None + self.robot_quaternion_x = None + self.robot_quaternion_y = None + self.robot_quaternion_z = None + self.robot_quaternion_w = None + + self.linear_velocity = 1.0 #1.4 + self.angular_velocity = 0.2 #0.14 + + + def lidar_callback(self, msg): + time.sleep(0.08) + min_distance = min(msg.ranges) + self.lidar = min_distance + if min_distance < self.min_distance: + self.stop_robot() + #time.sleep(2) ULTIMO RECURSO :) + self.COLLISION = True + print("COLISION!!!!!!!!!!!!") + if min_distance > self.min_distance: + self.COLLISION = False + + def odom_callback(self, msg): + if wp.digitalRead(start) == wp.LOW: + self.robot_x = msg.pose.pose.position.x + self.robot_y = msg.pose.pose.position.y + position = (self.robot_x, self.robot_y) + print("POSICION: ", position) + self.robot_quaternion_x = msg.pose.pose.orientation.x + self.robot_quaternion_y = msg.pose.pose.orientation.y + self.robot_quaternion_z = msg.pose.pose.orientation.z + self.robot_quaternion_w = msg.pose.pose.orientation.w + orientation_list = [msg.pose.pose.orientation.x ,msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) + self.current_angle = math.degrees(yaw) + print("ANGLE: ", self.current_angle) + target = self.path[self.current_path_index] + print("TARGET: ", self.current_path_index) + + + # if isinstance(target, Point): + # distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + # angle_to_target = (math.atan2(self.robot_x - target.x, self.robot_y - target.y) * 180) / math.pi + + # if abs(angle_to_target) > 90: + # direction = -1 + # else: + # direction = 1 + + # #giro + # if 0.3 < angle_to_target < -0.3: #el rango es menos al "recto" ya que el if está arriba, deja un margen por si nos desviamos al ir recto + # rotate_to_angle(angle_to_target) #piendo que self.angle es en grados y va de -180 a 180 + + # if -0.5 < angle_to_target < 0.5 and distance_to_target > self.distance_threshold: + # #hay que implemetar una función que detecte si la distacia aumenta, para parar + # self.move_towards_point(target) + + # else: + # self.current_path_index += 1 + # if self.current_path_index < len(self.path): + # self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + # else: + # self.get_logger().info("Reached the end of the path.") + # self.stop_robot() + + if isinstance(target, Point): + distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) + #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): + if distance_to_target > 0.1: + self.move_towards_point(target) + print("target distance: ", distance_to_target) + else: + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + + elif isinstance(target, Angle): + + angle_to_target = (target.angle - self.current_angle) + print("Tengo que girar: ", angle_to_target) + if abs(angle_to_target) > 0.5: + self.rotate_to_angle(target) + else: + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + elif isinstance(target, MyRio): + baud = 9600 + program = target.program + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, program) + char_val = wp.serialGetchar(serial) + + + if char_val == 102: + self.current_path_index += 1 + self.get_logger().info(f"SECUENCIA BRAZO TERMINADA {self.current_path_index}. Moving to next point...") + + + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") + + elif isinstance(target, SIMA): + baud = 9600 + code = target.code + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, code) + self.send_once = True + + if self.send_once == True: + self.current_path_index += 1 + + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") + + + def move_towards_point(self, target_point): + twist_msg = Twist() + angle_to_target_radians = math.atan2(target_point.y - self.robot_y, target_point.x - self.robot_x) + angle_to_target = np.degrees(angle_to_target_radians) - self.current_angle + distance_to_travel = math.sqrt((self.robot_x - target_point.x)**2 + (self.robot_y - target_point.y)**2) + + #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) + print("target rotation: ", angle_to_target) + + if abs(angle_to_target) <= 100: + new_angle_to_target = angle_to_target + direction = 1 + print("direction: 1") + + elif abs(angle_to_target) > 100: #para ir marcha atras + new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 + direction = -1 + print("direction: -1") + + print("new target rotation: ", new_angle_to_target) + #girar + if abs(new_angle_to_target) > 5 and self.COLLISION == False: #si el desvio es mayor a 2º, girar #margen de correcion + #twist_msg.angular.z = self.angular_velocity * (new_angle_to_target / (abs(new_angle_to_target) + 0.0000000000001)) + (0.005 * new_angle_to_target) + twist_msg.angular.z = PID_ang(0, new_angle_to_target) + print("girando") + self.cmd_vel_pub.publish(twist_msg) + self.test_var_1 = 0 + #avanzar + elif abs(new_angle_to_target) <= 5 and self.COLLISION == False: + twist_msg.angular.z = PID_ang(0, new_angle_to_target) * 0.01 + #twist_msg.linear.x = (self.linear_velocity * direction) - (0.1 * abs(new_angle_to_target)) + (distance_to_travel * 0.05 * direction) - ((distance_to_travel / 50) ** 2) #si empieza muy lento, reducir el numero de la division de distance_to_travel + twist_msg.linear.x = PID_lin(distance_to_travel) + print("avanzando") + self.cmd_vel_pub.publish(twist_msg) + self.test_var_1 = 5 + + + + + def rotate_to_angle(self, target_angle): + if self.COLLISION == False: + new_angle_to_target = target_angle.angle - self.current_angle + twist_msg = Twist() + twist_msg.angular.z = self.angular_velocity * (new_angle_to_target / (abs(new_angle_to_target) + 0.0000000000001)) + (0.005 * new_angle_to_target) + self.cmd_vel_pub.publish(twist_msg) + print("Operacion girado") + + + def stop_robot(self): + twist_msg = Twist() + twist_msg.linear.x = 0.0 + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + +class PID_ang: + def __init__(current_value, target_value): + Kp = 1 + Ki = 1 + Kd = 1 + + prev_time = time.time() + prev_error = 0 + integral = 0 + + def update(): + current_time = time.time() + dt = current_time - prev_time + error = current_value - target_value + derivative = error - prev_error + integral += error + output = Kp * error + Ki * integral + Kd * derivative + prev_error = error + return output + +class PID_lin: + def __init__(current_value, target_value): + Kp = 1 + Ki = 1 + Kd = 1 + + prev_time = time.time() + prev_error = 0 + integral = 0 + + def update(): + current_time = time.time() + dt = current_time - prev_time + error = current_value - target_value + derivative = error - prev_error + integral += error + output = Kp * error + Ki * integral + Kd * derivative + prev_error = error + return output + + +def main(args=None): + rclpy.init(args=args) + controller = RobotController() + rclpy.spin(controller) + rclpy.shutdown() + +if __name__ == '__main__': + main() From dd5242078ee0e806df56d6d4b73422ec91e9b184 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Tue, 7 May 2024 21:40:58 +0200 Subject: [PATCH 20/21] Create path_PID --- Eurobot_2024/path_PID | 359 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 359 insertions(+) create mode 100644 Eurobot_2024/path_PID diff --git a/Eurobot_2024/path_PID b/Eurobot_2024/path_PID new file mode 100644 index 0000000..8c9eb82 --- /dev/null +++ b/Eurobot_2024/path_PID @@ -0,0 +1,359 @@ +#BASIC PATH +import math +import time +import numpy as np +import wiringpi as wp +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, Point +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +from custom_msgs.msg import Angle, MyRio, SIMA # Import the custom Angle message +from tf_transformations import euler_from_quaternion + +wp.wiringPiSetupGpio() +start = 26 +side = 13 + +class RobotController(Node): + def __init__(self): + super().__init__('robot_controller') + self.cmd_vel_pub = self.create_publisher(Twist, 'diff_cont/cmd_vel_unstamped', 10) + self.odom_sub = self.create_subscription(Odometry, 'diff_cont/odom', self.odom_callback, 10) + self.subscription = self.create_subscription( + LaserScan, + '/scan', # Use '/scan' as the topic name + self.lidar_callback, + 10 + ) + if wp.digitalRead(side) == wp.LOW: #Lado amarillo + ########### el 3 es amarillo + ########### el 4 es azul + self.path = [ + SIMA(code = 'b'), + MyRio(program='4a'), + Point(x=-1.0, y=0.0), + Point(x=-1.0, y=-2.3), + Angle(angle = 0.0), + Point(x=0.0, y=-2.3), + MyRio(program='4a'), + Point(x=-1.0, y=-2.3), + Point(x=-1.0, y=-4.4), + Angle(angle = 0.0), + Point(x=0.0, y=-4.4), + MyRio(program='4a'), + Point(x=-1.0, y=-4.4), + #Point(x=-1.0, y=-10.0), + Point(x=-9.0, y=-13.0), + Angle(angle=135.0), + Point(x=-14.0, y=0.0), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=0.0), + # #Angle(angle=90.0), + # Point(x=-1.0, y=2.25), + # Angle(angle=0.0),ç + + # Point(x=0.0, y=2.25), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=2.25), + ########### el 3 es amarillo + # Point(x=-1.0, y=4.5), + # Angle(angle=0.0), + # Point(x=0.0, y=4.5), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=4.5), + # Point(x=-1.0, y=10.0), + # Angle(angle=0.0), + # Point(x=0.0, y=10.0), + # Angle(angle=0.0), + # MyRio(program='4a'), + # Point(x=-5.0, y=25.0), + # Point(x=-15.0, y=0.5), + # Angle(angle=135.0), + # Point(x=0.0, y=5.0), + # MyRio(program='4a'), + # Point(x=-6.0, y=5.0), + # Point(x=-6.0, y=0.0), + # Point(x=-1.0, y=0.0), + # Angle(angle=90.0), + # Point(x=-1.0, y=10.0), + # Angle(angle=0.0), + # Point(x=0.0, y=10.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=10.0), + # Angle(angle=-90.0), + # Point(x=-1.0, y=0.0), + ] + if wp.digitalRead(side) == wp.HIGH: #Lado azul + self.path = [ + Point(x=2.0, y=0.0), + Point(x=2.0, y=10.0), + Point(x=10.0, y=10.0), + Point(x=10.0, y=-10.0), + Point(x=2.0, y=-10.0), + Point(x=2.0, y=0.0), + Angle(0.0), + Point(x=0.0, y=0.0), + # #MyRio(program='4a'), + # Point(x=-1.0, y=0.0), + # #Angle(angle=90.0), + # Point(x=-1.0, y=-5.0), + # #Angle(angle=135.0), + # #Point(x=0.0, y=5.0), + # #MyRio(program='4a'), + # Point(x=-6.0, y=-5.0), + # Point(x=-6.0, y=0.0), + # Point(x=-1.0, y=0.0), + # Angle(angle=90.0), + # Point(x=-1.0, y=10.0), + # Angle(angle=0.0), + # Point(x=0.0, y=10.0), + # MyRio(program='4a'), + # Point(x=-1.0, y=10.0), + # Angle(angle=-90.0), + # Point(x=-1.0, y=0.0), + ] + + + + self.min_distance = 0.5 + self.COLLISION = False + + self.current_path_index = 0 + self.distance_threshold = 0.1 # Set your distance threshold for stopping + self.robot_x = None + self.robot_y = None + + self.send_once = False + + self.robot_angle = None + self.robot_quaternion_x = None + self.robot_quaternion_y = None + self.robot_quaternion_z = None + self.robot_quaternion_w = None + + self.linear_velocity = 1.0 #1.4 + self.angular_velocity = 0.2 #0.14 + + + def lidar_callback(self, msg): + time.sleep(0.08) + min_distance = min(msg.ranges) + self.lidar = min_distance + if min_distance < self.min_distance: + self.stop_robot() + #time.sleep(2) ULTIMO RECURSO :) + self.COLLISION = True + print("COLISION!!!!!!!!!!!!") + if min_distance > self.min_distance: + self.COLLISION = False + + def odom_callback(self, msg): + if wp.digitalRead(start) == wp.LOW: + self.robot_x = msg.pose.pose.position.x + self.robot_y = msg.pose.pose.position.y + position = (self.robot_x, self.robot_y) + print("POSICION: ", position) + self.robot_quaternion_x = msg.pose.pose.orientation.x + self.robot_quaternion_y = msg.pose.pose.orientation.y + self.robot_quaternion_z = msg.pose.pose.orientation.z + self.robot_quaternion_w = msg.pose.pose.orientation.w + orientation_list = [msg.pose.pose.orientation.x ,msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) + self.current_angle = math.degrees(yaw) + print("ANGLE: ", self.current_angle) + target = self.path[self.current_path_index] + print("TARGET: ", self.current_path_index) + + + # if isinstance(target, Point): + # distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + # angle_to_target = (math.atan2(self.robot_x - target.x, self.robot_y - target.y) * 180) / math.pi + + # if abs(angle_to_target) > 90: + # direction = -1 + # else: + # direction = 1 + + # #giro + # if 0.3 < angle_to_target < -0.3: #el rango es menos al "recto" ya que el if está arriba, deja un margen por si nos desviamos al ir recto + # rotate_to_angle(angle_to_target) #piendo que self.angle es en grados y va de -180 a 180 + + # if -0.5 < angle_to_target < 0.5 and distance_to_target > self.distance_threshold: + # #hay que implemetar una función que detecte si la distacia aumenta, para parar + # self.move_towards_point(target) + + # else: + # self.current_path_index += 1 + # if self.current_path_index < len(self.path): + # self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + # else: + # self.get_logger().info("Reached the end of the path.") + # self.stop_robot() + + if isinstance(target, Point): + distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) + #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): + if distance_to_target > 0.1: + self.move_towards_point(target) + print("target distance: ", distance_to_target) + else: + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + + elif isinstance(target, Angle): + + angle_to_target = (target.angle - self.current_angle) + print("Tengo que girar: ", angle_to_target) + if abs(angle_to_target) > 0.5: + self.rotate_to_angle(target) + else: + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + elif isinstance(target, MyRio): + baud = 9600 + program = target.program + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, program) + char_val = wp.serialGetchar(serial) + + + if char_val == 102: + self.current_path_index += 1 + self.get_logger().info(f"SECUENCIA BRAZO TERMINADA {self.current_path_index}. Moving to next point...") + + + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") + + elif isinstance(target, SIMA): + baud = 9600 + code = target.code + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, code) + self.send_once = True + + if self.send_once == True: + self.current_path_index += 1 + + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") + + + def move_towards_point(self, target_point): + twist_msg = Twist() + angle_to_target_radians = math.atan2(target_point.y - self.robot_y, target_point.x - self.robot_x) + angle_to_target = np.degrees(angle_to_target_radians) - self.current_angle + distance_to_travel = math.sqrt((self.robot_x - target_point.x)**2 + (self.robot_y - target_point.y)**2) + + #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) + print("target rotation: ", angle_to_target) + + if abs(angle_to_target) <= 100: + new_angle_to_target = angle_to_target + direction = 1 + print("direction: 1") + + elif abs(angle_to_target) > 100: #para ir marcha atras + new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 + direction = -1 + print("direction: -1") + + print("new target rotation: ", new_angle_to_target) + #girar + if abs(new_angle_to_target) > 5 and self.COLLISION == False: #si el desvio es mayor a 2º, girar #margen de correcion + #twist_msg.angular.z = self.angular_velocity * (new_angle_to_target / (abs(new_angle_to_target) + 0.0000000000001)) + (0.005 * new_angle_to_target) + twist_msg.angular.z = PID_ang(0, new_angle_to_target) + print("girando") + self.cmd_vel_pub.publish(twist_msg) + self.test_var_1 = 0 + #avanzar + elif abs(new_angle_to_target) <= 5 and self.COLLISION == False: + twist_msg.angular.z = PID_ang(0, new_angle_to_target) * 0.01 + #twist_msg.linear.x = (self.linear_velocity * direction) - (0.1 * abs(new_angle_to_target)) + (distance_to_travel * 0.05 * direction) - ((distance_to_travel / 50) ** 2) #si empieza muy lento, reducir el numero de la division de distance_to_travel + twist_msg.linear.x = PID_lin(distance_to_travel) + print("avanzando") + self.cmd_vel_pub.publish(twist_msg) + self.test_var_1 = 5 + + + + + def rotate_to_angle(self, target_angle): + if self.COLLISION == False: + new_angle_to_target = target_angle.angle - self.current_angle + twist_msg = Twist() + twist_msg.angular.z = self.angular_velocity * (new_angle_to_target / (abs(new_angle_to_target) + 0.0000000000001)) + (0.005 * new_angle_to_target) + self.cmd_vel_pub.publish(twist_msg) + print("Operacion girado") + + + def stop_robot(self): + twist_msg = Twist() + twist_msg.linear.x = 0.0 + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + +class PID_ang: + def __init__(current_value, target_value): + Kp = 1 + Ki = 1 + Kd = 1 + + prev_time = time.time() + prev_error = 0 + integral = 0 + + def update(): + current_time = time.time() + dt = current_time - prev_time + error = current_value - target_value + derivative = error - prev_error + integral += error + output = Kp * error + Ki * integral + Kd * derivative + prev_error = error + return output + +class PID_lin: + def __init__(current_value, target_value): + Kp = 1 + Ki = 1 + Kd = 1 + + prev_time = time.time() + prev_error = 0 + integral = 0 + + def update(): + current_time = time.time() + dt = current_time - prev_time + error = current_value - target_value + derivative = error - prev_error + integral += error + output = Kp * error + Ki * integral + Kd * derivative + prev_error = error + return output + + +def main(args=None): + rclpy.init(args=args) + controller = RobotController() + rclpy.spin(controller) + rclpy.shutdown() + +if __name__ == '__main__': + main() From c2245c778a0b971323f302eeb432de0c43564460 Mon Sep 17 00:00:00 2001 From: Liujw57 <117916080+Liujw57@users.noreply.github.com> Date: Thu, 9 May 2024 00:44:13 +0200 Subject: [PATCH 21/21] Create path_v3-1.py --- Eurobot_2024/path_v3-1.py | 319 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 319 insertions(+) create mode 100644 Eurobot_2024/path_v3-1.py diff --git a/Eurobot_2024/path_v3-1.py b/Eurobot_2024/path_v3-1.py new file mode 100644 index 0000000..34dc682 --- /dev/null +++ b/Eurobot_2024/path_v3-1.py @@ -0,0 +1,319 @@ +#Tiene: +#-Reseteo automático de encoders +#-Acceleración sueve +#BASIC PATH +import math +import time +import numpy as np +import wiringpi as wp +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, Point +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +from custom_msgs.msg import Angle, MyRio, SIMA # Import the custom Angle message +from tf_transformations import euler_from_quaternion + +wp.wiringPiSetupGpio() +start = 26 +side = 13 + +class RobotController(Node): + def __init__(self): + super().__init__('robot_controller') + self.odom_sub=self.create_subscription(Odometry, 'diff_cont/odom', self.odom_callback, 10) + #self.odom_pub =self.create_publisher(Odometry, 'diff_cont/odom', 10) + self.cmd_vel_pub = self.create_publisher(Twist, 'diff_cont/cmd_vel_unstamped', 10) + self.subscription = self.create_subscription( + LaserScan, + '/scan', # Use '/scan' as the topic name + self.lidar_callback, + 10 + ) + if wp.digitalRead(side) == wp.LOW: #Lado amarillo + ########### el 3 es amarillo + ########### el 4 es azul + self.path = [ + SIMA(code = 'b'), + Point(x=0.0 , y=0.0), + Point(x=-7.0 , y=7.0), + Point(x=-14.0 , y=0.0), + #Angle(angle=180.0), + Point(x=0.0 , y=0.0), + #MyRio(program='1a'), + # Point(x=-1.0, y=0.0), + # Point(x=-1.0, y=2.3), + # Angle(angle = 0.0), + # Point(x=0.0, y=-2.3), + + ] + if wp.digitalRead(side) == wp.HIGH: #Lado azul + self.path = [ + SIMA(code = 'b'), + Point(x=0.0 , y=0.0), + Point(x=7.0 , y=7.0), + Point(x=14.0 , y=0.0), + #Angle(angle=180.0), + Point(x=0.0 , y=0.0), + # Point(x=2.0, y=0.0), + # Point(x=2.0, y=10.0), + # Point(x=10.0, y=10.0), + # Point(x=10.0, y=-10.0), + # Point(x=2.0, y=-10.0), + # Point(x=2.0, y=0.0), + # Angle(0.0), + # Point(x=0.0, y=0.0), + + ] + + + + self.min_distance = 0.5 + self.COLLISION = False + + self.current_path_index = 0 + self.distance_threshold = 0.1 # Set your distance threshold for stopping + self.robot_x = None + self.robot_y = None + + self.send_once = False + + self.robot_angle = None + self.robot_quaternion_x = None + self.robot_quaternion_y = None + self.robot_quaternion_z = None + self.robot_quaternion_w = None + + self.correction_x = 0 + self.correction_y = 0 + self.correction_z = 0 + + self.correction_angular_total = 0 + + self.linear_velocity_max = 0.7 #1.4 + self.linear_acceleration = 0.01 + self.linear_speed = 0 + self.angular_velocity_max = 0.07 #0.14 + self.angular_acceleration = 0.001 + self.angular_speed = 0 + + def lidar_callback(self, msg): + time.sleep(0.08) + min_distance = min(msg.ranges[135:225]) + #self.lidar = min_distance + if min_distance < self.min_distance: + self.stop_robot() + #time.sleep(2) ULTIMO RECURSO :) + self.COLLISION = True + print("COLISION!!!!!!!!!!!!") + if min_distance > self.min_distance: + self.COLLISION = False + + def odom_callback(self, msg): + if wp.digitalRead(start) == wp.LOW: + self.robot_x = msg.pose.pose.position.x - self.correction_x + self.robot_y = msg.pose.pose.position.y - self.correction_y + position = (self.robot_x, self.robot_y) + print("POSICION: ", position) + self.robot_quaternion_x = msg.pose.pose.orientation.x + self.robot_quaternion_y = msg.pose.pose.orientation.y + self.robot_quaternion_z = msg.pose.pose.orientation.z + self.robot_quaternion_w = msg.pose.pose.orientation.w + orientation_list = [self.robot_quaternion_x , self.robot_quaternion_y, self.robot_quaternion_z, self.robot_quaternion_w] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) + self.current_angle = math.degrees(yaw) - self.correction_angular_total + print("ANGLE: ", self.current_angle) + target = self.path[self.current_path_index] + print("TARGET: ", self.current_path_index) + + + + + # if isinstance(target, Point): + # distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + # angle_to_target = (math.atan2(self.robot_x - target.x, self.robot_y - target.y) * 180) / math.pi + + # if abs(angle_to_target) > 90: + # direction = -1 + # else: + # direction = 1 + + # #giro + # if 0.3 < angle_to_target < -0.3: #el rango es menos al "recto" ya que el if está arriba, deja un margen por si nos desviamos al ir recto + # rotate_to_angle(angle_to_target) #piendo que self.angle es en grados y va de -180 a 180 + + # if -0.5 < angle_to_target < 0.5 and distance_to_target > self.distance_threshold: + # #hay que implemetar una función que detecte si la distacia aumenta, para parar + # self.move_towards_point(target) + + # else: + # self.current_path_index += 1 + # if self.current_path_index < len(self.path): + # self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + # else: + # self.get_logger().info("Reached the end of the path.") + # self.stop_robot() + + if isinstance(target, Point): + distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) + #distance_to_target = math.sqrt((self.robot_x - target.x)**2 + (self.robot_y - target.y)**2) * ((self.robot_x - target.x) / abs(self.robot_x - target.x)) * ((self.robot_y - target.y) / abs(self.robot_y - target.y)) + #if distance_to_target > self.distance_threshold and ((prev_dist - distance_to_target) > 0): + if distance_to_target > 0.1: + self.move_towards_point(target) + print("target distance: ", distance_to_target) + else: + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + + elif isinstance(target, Angle): + + angle_to_target = (target.angle - self.current_angle) + print("Tengo que girar: ", angle_to_target) + if abs(angle_to_target) > 0.5: + self.rotate_to_angle(target) + else: + self.current_path_index += 1 + if self.current_path_index < len(self.path): + self.get_logger().info(f"Reached point {self.current_path_index}. Moving to next point...") + else: + self.get_logger().info("Reached the end of the path.") + self.stop_robot() + + elif isinstance(target, MyRio): + baud = 9600 + program = target.program + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, program) + char_val = wp.serialGetchar(serial) + + + if char_val == 102: + self.current_path_index += 1 + self.get_logger().info(f"SECUENCIA BRAZO TERMINADA {self.current_path_index}. Moving to next point...") + + + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") + + elif isinstance(target, SIMA): + baud = 9600 + code = target.code + serial = wp.serialOpen("/dev/serial0", baud) + wp.serialPuts(serial, code) + self.send_once = True + + if self.send_once == True: + self.current_path_index += 1 + + else: + self.get_logger().info(f"MOVIENDO BRAZO CON EL PROGRAMA {program}") + + + elif wp.digitalRead(start) == wp.HIGH: + self.current_path_index = 0 + + self.correction_x = msg.pose.pose.position.x + self.correction_y = msg.pose.pose.position.y + self.correction_z = msg.pose.pose.position.z + + self.robot_x = msg.pose.pose.position.x - self.correction_x + self.robot_y = msg.pose.pose.position.y - self.correction_y + position = (self.robot_x, self.robot_y) + self.robot_quaternion_x = msg.pose.pose.orientation.x + self.robot_quaternion_y = msg.pose.pose.orientation.y + self.robot_quaternion_z = msg.pose.pose.orientation.z + self.robot_quaternion_w = msg.pose.pose.orientation.w + orientation_list = [self.robot_quaternion_x , self.robot_quaternion_y, self.robot_quaternion_z, self.robot_quaternion_w] + (roll, pitch, yaw) = euler_from_quaternion(orientation_list) + self.correction_angular_total = math.degrees(yaw) + + + def move_towards_point(self, target_point): + twist_msg = Twist() + angle_to_target_radians = math.atan2(target_point.y - self.robot_y, target_point.x - self.robot_x) + angle_to_target = np.degrees(angle_to_target_radians) - self.current_angle + distance_to_travel = math.sqrt((self.robot_x - target_point.x)**2 + (self.robot_y - target_point.y)**2) + + + #target_vector = (target_point.x - self.robot_x, target_point.y - self.robot_y) + print("target rotation: ", angle_to_target) + + if abs(angle_to_target) <= 100: + new_angle_to_target = angle_to_target + direction = 1 + print("direction: 1") + + elif abs(angle_to_target) > 100: #para ir marcha atras + new_angle_to_target = (180 - abs(angle_to_target)) * (angle_to_target / abs(angle_to_target)) * -1 + direction = -1 + print("direction: -1") + + speed_target = min(distance_to_travel * self.linear_acceleration, self.linear_velocity_max) + angular_dir = (new_angle_to_target / abs(new_angle_to_target)) + speed_target_angle = min(abs(new_angle_to_target * self.angular_acceleration), self.angular_acceleration) + + print("new target rotation: ", new_angle_to_target) + #girar + if abs(new_angle_to_target) > 5 and self.COLLISION == False: #si el desvio es mayor a 2º, girar #margen de correcion + while abs(new_angle_to_target) > 3 and self.COLLISION == False: + twist_msg.angular.z = self.angular_velocity * (new_angle_to_target / (abs(new_angle_to_target) + 0.0000000000001)) + (0.005 * new_angle_to_target) + + # ******Está por terminar, no se como hacerlo, por los malditos signos (T.T ), buena suerte crack + # angular_dir = () + + # if abs(self.angular_speed) < speed_target_angle: + # self.angular_speed += self.angular_acceleration + # elif abs(self.angular_speed) > speed_target_angle: + # self.angular_speed -= self.angular_acceleration + + + print("girando") + self.cmd_vel_pub.publish(twist_msg) + + #avanzar + elif abs(new_angle_to_target) <= 5 and self.COLLISION == False: + twist_msg.angular.z = new_angle_to_target * 0.01 + #twist_msg.linear.x = (self.linear_velocity * direction) - (0.1 * abs(new_angle_to_target)) + (distance_to_travel * 0.05 * direction) - ((distance_to_travel / 50) ** 2) #si empieza muy lento, reducir el numero de la division de distance_to_travel + + #algo raro de la velocidad, tocar aqui + if self.linear_velocity < speed_target: + self.linear_velocity += self.linear_acceleration + elif self.linear_velocity > speed_target: + self.linear_velocity -= self.linear_acceleration + + twist_msg.linear.x = self.linear_velocity * direction + print("avanzando") + self.cmd_vel_pub.publish(twist_msg) + + + + + def rotate_to_angle(self, target_angle): + if self.COLLISION == False: + new_angle_to_target = target_angle.angle - self.current_angle + twist_msg = Twist() + twist_msg.angular.z = self.angular_velocity * (new_angle_to_target / (abs(new_angle_to_target) + 0.0000000000001)) + (0.005 * new_angle_to_target) + self.cmd_vel_pub.publish(twist_msg) + print("Operacion girado") + + + + def stop_robot(self): + twist_msg = Twist() + twist_msg.linear.x = 0.0 + twist_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(twist_msg) + +def main(args=None): + rclpy.init(args=args) + controller = RobotController() + rclpy.spin(controller) + rclpy.shutdown() + +if __name__ == '__main__': + main()