Skip to content

Commit

Permalink
<feat> [435-task-velocity-controller] Implemented stable PI control f…
Browse files Browse the repository at this point in the history 
…or pitch and yaw, still working on surge and heave.
  • Loading branch information
@Q3rkses
Q3rkses committed Sep 29, 2024
1 parent 87e0576 commit 35318d4
Showing 1 changed file with 55 additions and 30 deletions.
85 changes: 55 additions & 30 deletions control/velocity_controller_c/scripts/velocity_controller_c.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
from std_msgs.msg import Float32MultiArray



#A function to convert orientation quaternions to euler angles
def quaternion_to_euler_angle(w, x, y, z):
ysqr = y * y
Expand Down Expand Up @@ -37,6 +36,18 @@ def ssa(angle):
angle += 2 * np.pi
return angle

def rotate_point(x, y, z , theta, phi, psi):
# Rotation matrix
R = np.array([[np.cos(theta)*np.cos(psi), np.sin(phi)*np.sin(theta)*np.cos(psi) - np.cos(phi)*np.sin(psi), np.cos(phi)*np.sin(theta)*np.cos(psi) + np.sin(phi)*np.sin(psi)],
[np.cos(theta)*np.sin(psi), np.sin(phi)*np.sin(theta)*np.sin(psi) + np.cos(phi)*np.cos(psi), np.cos(phi)*np.sin(theta)*np.sin(psi) - np.sin(phi)*np.cos(psi)],
[-np.sin(theta), np.sin(phi)*np.cos(theta), np.cos(phi)*np.cos(theta)]])
# Point to be rotated
P = np.array([x, y, z])
# Rotated point
P_rot = np.dot(R, P)

return round(P_rot[0],12) , round(P_rot[1],12), round(P_rot[2],12)

class VelocityNode(Node):

def __init__(self):
Expand All @@ -46,15 +57,15 @@ def __init__(self):
)

self.guidance_subscriber = self.create_subscription(
Float32MultiArray, "velocity_points", self.guidance_callback, 10
Float32MultiArray, "/guidance/los", self.guidance_callback, 10
)

self.publisher_controller = self.create_publisher(Wrench, "/thrust/wrench_input", 10)
self.publisher_states = self.create_publisher(Float32MultiArray, "/Velocity/States", 10)
self.control_timer = self.create_timer(0.1, self.publish_callback)
self.state_timer = self.create_timer(0.3, self.publish_states)
self.topic_subscriber = self.create_subscription(
Odometry, "/nucleus/odom", self.subscribe_callback, 10)
Odometry, "/nucleus/odom", self.nucleus_callback, 10)
self.publisher = self.create_publisher(Wrench, "/thrust/wrench_input",
10)
self.timer = self.create_timer(0.1, self.publish_callback)
Expand All @@ -72,24 +83,29 @@ def __init__(self):
self.position = np.array([0.0, 0.0, 0.0])
self.twist = np.array([0.0, 0.0, 0.0])
self.orientation = np.array([0.0, 0.0, 0.0])
self.abu_values = np.array([-0.2, -5.0 , -np.pi/4, np.pi/4])
self.rotated_point_position = np.array([0.0, 0.0, 0.0])
self.abu_values = np.array([0.3, -5.0 , np.pi/4, np.pi/4])

self.Kp_linear = 100.0
self.Kp_pitch = 2.0
self.Kp_yaw = 2.0
self.Kp_surge = 20.0
self.Kp_heave = 10.0
self.Kp_pitch = 2.5
self.Kp_yaw = 2.5

self.Ti_pitch = 0.05
self.Ti_yaw = 0.05
self.Ti_surge = 33.0
self.Ti_yaw = 0.001
self.Ti_surge = 1.0
self.Ti_heave = 0.5
self.pitch_integral_sum = 0.0
self.yaw_integral_sum = 0.0
self.surge_integral_sum = 0.0
self.heave_integral_sum = 0.0

self.heave_error = 0.0
self.pitch_error = 0.0
self.yaw_error = 0.0
self.surge_error = 0.0


self.get_logger().info("Velocity Controller Node has been started")

def nucleus_callback(self, msg: Odometry): # callback function
self.position = np.array(
Expand All @@ -114,41 +130,50 @@ def nucleus_callback(self, msg: Odometry): # callback function
msg.pose.pose.orientation.z)
self.orientation = np.array([X, Y, Z])

# self.rotated_point_position[0], self.rotated_point_position[1], self.rotated_point_position[2] = rotate_point(self.position[0], self.position[1], self.position[2], Y, X, Z)

def guidance_callback(self, msg: Float32MultiArray): # callback function
self.abu_values = np.array(msg.data[:3])
self.abu_values = msg.data


def publish_callback(self): # The controller function

self.surge_error = self.abu_values[0] - self.twist[0]
self.heave_error = self.abu_values[1] - self.rotated_point_position[2]
self.pitch_error = ssa(self.abu_values[2] - self.orientation[1])
self.yaw_error = ssa(self.abu_values[3] - self.orientation[2])

msg = Wrench()

if abs(self.surge_error) < 0.005:
self.surge_integral_sum = 0.0
# if abs(self.pitch_error) < 0.05 and abs(self.yaw_error < 0.05):

# if abs(self.surge_error) > 0.005:
# self.surge_integral_sum += self.surge_error
# else:
# self.surge_integral_sum += 0.0
# msg.force.x = self.surge_error*self.Kp_surge + self.surge_integral_sum * self.Ti_surge # SURGE

# else:
# self.surge_integral_sum = 0.0
# msg.force.x = 0.0

self.pitch_integral_sum += self.pitch_error
msg.torque.y = -(self.pitch_error * self.Kp_pitch + self.pitch_integral_sum * self.Ti_pitch) # PITCH

if abs(self.yaw_error) > 0.005:
self.yaw_integral_sum += self.yaw_error
else:
self.surge_integral_sum += self.surge_error
self.yaw_integral_sum = 0.0

msg.force.x = self.surge_error*self.Kp_linear + self.surge_integral_sum * self.Ti_surge # SURGE
msg.torque.z = -(self.yaw_error * self.Kp_yaw + self.yaw_integral_sum * self.Ti_yaw) # YAW

# if abs(self.heave_error) > 0.005:
# self.heave_integral_sum += self.heave_error
# else:
# self.heave_integral_sum = 0.0
# msg.force.z = self.heave_error*self.Kp_heave + self.heave_integral_sum * self.Ti_heave

msg.force.z = 0.0 # HEAVE
msg.torque.z = 0.0
msg.torque.y = 0.0

# if self.pitch_error < 0.4:
# self.pitch_integral_sum += self.pitch_error

# if self.yaw_error < 0.1:
# self.yaw_integral_sum += self.yaw_error

# msg.torque.y = 0.0
# msg.torque.y = -(self.pitch_error * self.Kp_pitch + self.pitch_integral_sum * self.Ti_pitch) # PITCH

# msg.torque.z = -(self.yaw_error * self.Kp_yaw + self.yaw_integral_sum * self.Ti_yaw)# YAW
# self.publisher_controller.publish(msg)
self.publisher_controller.publish(msg)

def publish_states(self):
msg = Float32MultiArray()
Expand Down

0 comments on commit 35318d4

Please sign in to comment.