Velocity Controller - riplaboratory/Kanaloa GitHub Wiki
- Idea
- Code
- Issues & Improvements
- Extras
The Velocity Controller is being designed with the same concepts as a PID controller. This velocity controller is meant to control the movement of the WAM-V. A PID controller has 3 aspects, a proportional, an integral, and a derivative aspect. The PID controls the movement of the WAM-V by adjusting its speed as it gets closer to its target velocity input. This will be beneficial to the WAM-V because it will be able to move in a stable and controlled manner. It is able to reach and maintain an input velocity in the surge and yaw directions. The sway direction will be added in later since being able to go forward and turn are higher priority than being able to strafe.
#!/usr/bin/env python
import sys
import rospy
from geopy.distance import geodesic
from std_msgs.msg import Float32
#ros topic info for the wamv/thrusters/right_thrust_cmd is type std_msgs/Float32 same for the left_thrust_cmd
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
#ros topic info for the wamv/sensors/gps/gps/fix is type sensor_msgs/NavSatFix
previous_error = 0
error_sum = 0
count = 0
previous_latitude = 0
previous_longitude = 0
angular_velocity_error_sum = 0
def velocity_output(fix, args):
global previous_error
global error_sum
global count
global previous_latitude
global previous_longitude
left_thruster = args[0]
right_thruster = args[1]
target_velocity = args[2]
start_location = args[3]
#distance traveled in kilometers
if previous_latitude == 0 and previous_longitude == 0:
kms_traveled = geodesic((start_location.latitude, start_location.longitude), (fix.latitude, fix.longitude)).km
else:
kms_traveled = geodesic((previous_latitude, previous_longitude), (fix.latitude, fix.longitude)).km
meters_traveled = kms_traveled * 1000 #meters traveled calculation
meters_per_second = meters_traveled * 10.0 #meters per second calculation
rospy.loginfo("Speed: %s m/sec", meters_per_second)
#write speed over time to a file
f = open("output.txt", "a")
f.write(str(count) + "," + str(meters_per_second) + "\n")
f.close()
#end of writing to a file
error = abs(target_velocity) - meters_per_second
kd = (error - previous_error) * 16 #derivative the change in time is 1/16th a second
#rospy.loginfo("derivative value: %s\nprevious error: %s", kd, previous_error)
previous_error = error
#rospy.loginfo("Current Error: %s", error)
if target_velocity > 0:
direction = 1.0
else:
direction = -1.0
kp = 5 #proportional gain
error_sum = error_sum + error
ki = error_sum #integral
#rospy.loginfo("Error Sum: %s", error_sum)
#rospy.loginfo('------------------------------------------------------------------------------')
if error > 0:
left_thruster.publish(direction * (kp * error) + (.05*ki) + (.1*kd))
right_thruster.publish(direction * (kp * error) + (.05*ki) + (.1*kd))
count = count + 1
previous_latitude = fix.latitude
previous_longitude = fix.longitude
#rospy.loginfo('Previous Latitude: %s', previous_latitude)
#rospy.loginfo('Previous Longitude: %s', previous_longitude)
#rospy.loginfo('------------------------------------------------------------------------------')
def angle_output(Imu, args):
global angular_velocity_error_sum
target_z_vel = args[0]
left_thruster_angle = args[1]
right_thruster_angle = args[2]
rospy.loginfo('target z vel: %s', target_imu_z)
rospy.loginfo('current imu z: %s', Imu.angular_velocity.z)
angular_velocity_error = target_z_vel - Imu.angular_velocity.z
magnitude = ((50 * angular_velocity_error) + (angular_velocity_error_sum * 3))
left_thruster_angle.publish(-1 * magnitude) # if you want a negative angular velocity, you want to angle your thrusters with a postive magnitude
right_thruster_angle.publish(-1* magnitude) # if you want a positive angular velocity, you want to angle your thrusters with a negative magnitude
rospy.loginfo('magnitude input: %s', magnitude)
angular_velocity_error_sum = angular_velocity_error_sum + (angular_velocity_error / 10)
rospy.loginfo('------------------------------------------------------------------------------')
def thrust(target_velocity, angular_velocity): #the input is the target velocity
rospy.init_node('thrust', anonymous=True)
left_thruster = rospy.Publisher('/wamv/thrusters/left_thrust_cmd', Float32, queue_size=10)
right_thruster = rospy.Publisher('/wamv/thrusters/right_thrust_cmd', Float32, queue_size=10)
left_thruster_angle = rospy.Publisher('/wamv/thrusters/left_thrust_angle', Float32, queue_size=10)
right_thruster_angle = rospy.Publisher('/wamv/thrusters/right_thrust_angle', Float32, queue_size=10)
start_location = rospy.wait_for_message('wamv/sensors/gps/gps/fix', NavSatFix)
rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, velocity_output, (left_thruster, right_thruster, target_velocity, start_location))
#rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, angle_output, (angular_velocity, left_thruster_angle, right_thruster_angle))
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rate.sleep()
if __name__ == '__main__': #when script is run, this is what is run after the imports. The thrust function is called and it takes 2 inputs.
try:
thrust(float(sys.argv[1]), float(sys.argv[2]))
except rospy.ROSInterruptException:
pass
Some issues with the code are, there are so safeguards put in place in the case where an input is given that the thrusters are incapable of outputting. Another issue is when both the surge and yaw inputs are inputted and both subscribers are active, the WAM-V tries to maintain speeds in both directions. This is not a huge issue if the WAM-V is moving very slowly for example .2 or .1 meters per second. However, once those values increase or get further apart for example an input of .7 m/s for surge and .2 m/s for yaw, the WAM-V tries to get to an maintain a .7 m/s moving forward and at the same time tries to get to and maintain a .2 m/s in yaw. From testing, this makes the WAM-V's movements unstable. A solution for this issue that is going to be tested is when this happens, have one thruster listen to the surge input and have the other thruster listen to the yaw input so one thruster is not trying to do both actions at the same time. A drawback is both thrusters would have to work twice as hard to compensate for only one thruster listening to the action instead of two. This is just an idea and has yet to be tested.
Here is the code for the thrust plot that is generated when running the thrust function of the code. Remember to remove the output.txt file before you run the function again, otherwise your subsequent running of the function will append to the output.txt file and make your plot difficult to read.
import matplotlib.pyplot as plt
import numpy as np
x, y = np.loadtxt('output.txt', delimiter=',', unpack=True)
plt.plot(x,y,label='Speed m/s')
plt.xlabel('Time (1/16 sec)')
plt.ylabel('Speed')
plt.title('Speed vs. Time')
plt.legend()
plt.show()