• Idea
• Proportional
• Integral
• Derivative
• Code
• Extras

The PID controller is meant to control the movement of the WAMV. The PID controller has 3 aspects, a proportional, an integral, and a derivative aspect. The PID controls the movement of the WAMV by adjusting its speed as it gets closer to its destination. This will be beneficial to the WAMV because it will be able to move and get to its destination in a timely manner and be accurate when it needs to go to a specific spot.

The proportional aspect of the PID controller is what mainly controls its speed within my code. An example of the proportional aspect would look like this.

error = target_distance - current_distance
left_thruster.publish(error)
right_thruster.publish(error)

In this code, the variable error is calculated by subtracting the current distance (the distance from the current GPS position to the target GPS position) from the target distance (the distance that is inputted for the WAMV to travel). This variable error is then put in as a value to the left and right thruster publishers making the WAMV move. Since the thrusters are not angled, the WAMV would just move in a straight path. As the WAMV gets closer to the target distance, it will decrease in speed since the error value will be getting smaller the closer to the target distance the WAMV gets. An issue that will come up with this is eventually the WAMV's speed will decrease to a point where the current pushing against it will match its speed. This means that the WAMV will effectively "stop" since the force pushing against it will be equal to the force pushing it forward. A way to remedy this problem is to include another aspect to the controller.

The integral aspect of the PID controller is meant to keep track of the error over time. Since the WAMV would be in steady-state when its speed matches the current, the integral of the error would an increasing value that would be added on to the speed of the WAMV to help it get to its target distance. This is the idea of the integral aspect of the PID but what is being used here is a little different.

boost = 1.0
if (speed < (.01 * error)) and (error < 0.5):
    speed_up = speed_up + .002
elif speed > (.2 * error):
    boost = 0.0
elif speed < (.1 * error):
    boost = 5.0
left_thruster.publish(boost * error)
right_thruster.publish(boost * error)

This is the block of code that is being used. There is a variable called boost, the same error variable from before, and a speed variable involved. We are ignoring the speed_up variable for now since that will be discussed later.

The speed variable is calculated by subtracting the last distance from the current distance and multiplying by 15. The last distance variable is the current distance value from a previous iteration. This variable is calculated 15 times a second, so multiplied by 15 because the amount of iterations per second is 15. This value gives the speed of the WAMV in meters per second. It is important to know the speed of the WAMV because that along with knowing the WAMV's distance away from the target distance, you can add a gain factor to increase the speed when the WAMV is stuck. The code is structured so if I am going to fast and very close to the goal the boost will go from being 1 to 0. When the boost is 0, this stops publishing to the left and right thrusters and just has the WAMV drift in its current direction. This is meant to avoid overshooting the target. When my speed drops below one-tenth of the error, the boost variable changes from 1 to 5. This gives the WAMV a push that will have it reach the goal and no longer be stuck. With this code, the WAMV is able to travel and be within 6 centimeters of its goal.

The derivative aspect of the PID is meant to decrease the speed when we are fast approaching the goal. When we are fast approaching our goal, the error is quickly decreasing and that means there is a negative rate of change. This negative value is meant to be added to the controller's output and will decrease our speed. In the last section where the boost variable is changed from 1 to 0 when the WAMV is fast approaching the target distance is an example of this idea, but not a complete application since there are no derivatives taken. There is recognition that the error is quickly decreasing by checking that our speed is two-tenths times the error and because of that, the speed decreases.

#!/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
#ros topic info for the wamv/sensors/gps/gps/fix is type sensor_msgs/NavSatFix

last_distance = 0.0
speed_up = 0.0
speed_back = 0.0
def proportional_movement(fix, args):
    global last_distance 
    global speed_up
    global speed_back
    left_thruster = args[0]
    right_thruster = args[1]
    target_distance = args[2]
    start_location = args[3]
    current_distance = geodesic((start_location.latitude, start_location.longitude), (fix.latitude, fix.longitude)).km
    current_distance = current_distance * 1000
    rospy.loginfo(current_distance)
    speed = (current_distance - last_distance) * 15
    rospy.loginfo("%s m/sec", speed)
    last_distance = current_distance
    error = abs(target_distance) - current_distance

    if target_distance > 0:
        direction = 1.0
    else:
        direction = -1.0

    if error >= 10:
        rospy.loginfo('error over 10 statement')
        left_thruster.publish(direction)
        right_thruster.publish(direction)
    elif error >= 0:
        boost = 1.0
        if (speed < (.01 * error)) and (error < 0.5):
            speed_up = speed_up + .002
        elif speed > (.2 * error):
            boost = 0.0
        elif speed < (.1 * error):
            boost = 5.0
        
        rospy.loginfo('less than 10 statement')
        rospy.loginfo(boost)
        rospy.loginfo('speedup = %s', speed_up)
        left_thruster.publish((direction * 0.075 * boost * (error)) + (direction * speed_up))
        right_thruster.publish((direction * 0.075 * boost * (error)) + (direction * speed_up))
    else:
        rospy.loginfo('else statement')
        if (speed < (.1 * abs(error))) and (error < -0.02):
            speed_back = speed_back + .002
        rospy.loginfo('speedback = %s', speed_back)
        left_thruster.publish((direction * 0.1 * error) - (direction * speed_back))
        right_thruster.publish((direction * 0.1 * error) - (direction * speed_back))

def move(target_distance): #the input is the target distance
    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)
    start_location = rospy.wait_for_message('wamv/sensors/gps/gps/fix', NavSatFix)
    rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, proportional_movement, (left_thruster, right_thruster, 
    target_distance, start_location))
    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 move function is called and it takes 1 input.
    try:
        move(float(sys.argv[1]))
    except rospy.ROSInterruptException:
        pass

To make the WAMV more accurate, a speed_up and speed_back variable has been created. The purpose of these variables is to help the WAMV with staying within a certain error when the WAMV reaches its destination. The speed_up variable starts incrementing only when the speed of the WAMV drops below one-tenth of the error and the error is less than half a meter. This is meant to keep the WAMV in a "hover" state where the speed pushing forward is equal to the current pushing back. In the case where this speed up overshoots the target distance, that is where the speed_back variable gets implemented. The speed_back variable starts incrementing when the speed is less than one-tenth of the error, and the error is less than 20 centimeters. This is meant to push the WAMV back to the target. This cycle happens to keep the WAMV at the target with an error of 2 centimeters.