Update original.py

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()