Keep velocity in determined value [quadcopter]
I need to test my slow PID controller but my quadcopter is not too stable, so I don't know if it's moving because of my PID or because of it's own movement. I think that the best way to do that is to make steps like this: https://imgur.com/yQAbmUY (I can't paste the picture because I have less than 5 points, sorry)
The output of the controller are the velocities in x, y and z (ux, uy and uz). In the time-lapse of the PID (for example, 0.2 seconds) I want the ux, uy and uz to have a determined value, and then in the time-lapse of the stop I want them to be ux=0.0, uy=0.0 and uz=0.0.
So my code would be something like this:
#!/usr/bin/env python
# a lot of imports
class gonnaGetDown():
def __init__(self):
self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.orientation_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
print "b"
global delta_t
global u_x_k, u_x_k_pre
global kp_x, ki_x, kd_x
global Ti_x, Td_x
global error_x_k, error_x_k_pre, error_x_k_pre_x2
global setpoint_x
global u_y_k, u_y_k_pre
global kp_y, ki_y, kd_y
global Ti_y, Td_y
global error_y_k, error_y_k_pre, error_y_k_pre_x2
global setpoint_y
global u_z_k, u_z_k_pre
global kp_z, ki_z, kd_z
global Ti_z, Td_z
global error_z_k, error_z_k_pre, error_z_k_pre_x2
global setpoint_z
delta_t = 1/3
setpoint_x = 0
setpoint_y = 0
setpoint_z = 1
#x:
error_x_k, error_x_k_pre, error_x_k_pre_x2 = 0, 0, 0
u_x_k = 0
#y:
error_y_k, error_y_k_pre, error_y_k_pre_x2 = 0, 0, 0
u_y_k = 0
#z:
error_z_k, error_z_k_pre, error_z_k_pre_x2 = 0, 0, 0
u_z_k = 0
print "b"
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.detect_marker)
def detect_marker(self, msg):
print "c"
global u_x_k, u_x_k_pre, u_y_k, u_y_k_pre, u_z_k, u_z_k_pre
global kp_x, ki_x, kd_x, kp_y, ki_y, kd_y, kp_z, ki_z, kd_z
global delta_t
global error_x_k, error_x_k_pre, error_x_k_pre_x2, error_y_k, error_y_k_pre, error_y_k_pre_x2, error_z_k, error_z_k_pre, error_z_k_pre_x2
global contador
Ti_x, Td_x = kp_x/ki_x, kd_x/kp_x
Ti_y, Td_y = kp_y/ki_y, kd_y/kp_y
Ti_z, Td_z = kp_z/ki_z, kd_z/kp_z
if not msg.markers:
print "Tag not found"
vel_msg = Twist()
vel_msg.linear.x = 0.0
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = 0.0
u_x_k = 0.0
u_y_k = 0.0
u_z_k = 0.0
self.velocity_publisher.publish(vel_msg)
return
else:
marker = msg.markers[0]
print "d"
#Primero ajusto las posiciones x e y con un PID
error_x_k_pre_x2 = error_x_k_pre
error_x_k_pre = error_x_k
error_x_k = setpoint_x - marker.pose.pose.position.x
error_y_k_pre_x2 = error_y_k_pre
error_y_k_pre = error_y_k
error_y_k = setpoint_y - marker.pose.pose.position.y
error_z_k_pre_x2 = error_z_k_pre
error_z_k_pre = error_z_k
error_z_k = setpoint_z - marker.pose.pose.position.z
u_x_k = ... # algorithm
u_y_k = ... # algorithm
u_z_k = ... # algorithm
vel_msg = Twist()
vel_msg.linear.x = u_y_k
vel_msg.linear.y = u_x_k
vel_msg.linear.z = u_z_k
self.velocity_publisher.publish(vel_msg)
time.sleep(0.2) #0.2 seconds
vel_msg.linear.x = 0.0
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
self.velocity_publisher.publish(vel_msg)
time.sleep(0.7) #0.7 seconds
if __name__ == '__main__':
rospy.init_node('go_down', anonymous=True)
try:
rate = rospy.Rate(0.1)
gonna_get_down = gonnaGetDown()
while not rospy.is_shutdown ...
Are you asking how to write a PID controller?
No. I'm asking how to keep the velocity (vel_msg.linear.x, for example) in a determined value for a period of time. For example, I want to keep the vel_msg.linear.x=0.0 for 0.7 seconds. How can I do that? I imagine that a for loop can do that but the computational cost would be high
I mean, I want to update vel_msg.linear.x multiple times for a period of time, so if there's an external disturbance or something, it keeps the same value (if I put vel_msg.linear.x=0.0 and I hit the quadcopter, the vel_msg.linear.x would be different than 0.0 and wouldn't return to 0.0)
So, you're only talking about publishing messages for certain periods and not maintaining your quadcopter at a certain velocity then?
No. I want to maintain the velocity of the quadcopter at a determined value for a period of time. For example, if I want the velocity is 0.2, then I want the velocity to be 0.2 in all that time-lapse. If I hit the quadcopter from behind at make it higher, then it slows down, and viceversa
You want to write some kind of controller then
No. For example, a for loop that lasts 1 second would be a solution, because it would post the same velocity for 1 second. But how many iterations do I have to do in 1 second? And it would be really expensive computationally, I suppose
If you're wanting to maintain a certain velocity, then you want a controller. Simply publishing a message telling your quadcopter to go a certain velocity won't cut it (unless you happen to be very lucky and it just happens to work out).