I wanted to create an auto-pilot algorithm that would allow you set a desired heading between 0-360 degrees and then at some interval supply the ROV's current heading to the autopilot in order to get back the port and starboard thruster power levels. How the port and thruster power levels are calculated I guess I consider to be "the algorithm".
Here is a web page that shows the auto-pilot logic in action: http://www.vincentfurniture.com/openrov/openrov%20autopilot.html
Here's how the overall auto-pilot process works:
Set a Desired Heading between 0 and 360 degrees. 0 being North, 90 being east, 180 south, and 270 West
Set a buffer angle, i.e 10 degrees. This buffer angle is used to help guide the ROV into the heading. If your desired heading is 90, then headings from 80 to 100 degress are considered in the algorithm. Recommended buffer angle ranges are between 3 to 10.
Once a desired heading has been set call autopilot.updateHeading() with the latest ROV heading between 0 and 360.
The updateHeading method returns an object with portPower and starboardPower properties that have a value between -1.0 and 1.0 with a step of .001. A value of 1 is 100% full forward power and -1 represents 100% reverse power.
The autopilot calculates the port and starboard power using the following process:
- Calculate the two angles between latest supplied heading and the desired heading
- Choose the smaller of the two angles as the direction to turn the ROV. The smallest angle will always be less than or equal to 180. The two angles always add up to 360.
- The smallest angle difference is stored as angle_diff
- Apply power to motors in opposite directions to rotate ROV to face the desired heading
- Calculate power applied to motors based on how far off of heading
i. More power to motors the further off the heading (to turn quickly)
ii. Less power the closer to the desired heading (so as to not overshoot)
- Spin motors in opposite direction until within certain number of degrees (buffer angle) of desired heading
i. Example, assuming buffer angle is 10 degrees. If the desired heading is 100, and latest heading is 20 motors will be spun in opposite directions until current heading is greater than 90
- Once the angle_diff is less than the buffer angle range apply forward power to both motors
- Two conditions of forward power
i. If angle_diff is greater than half of buffer angle but less than buffer angle (5<angle_diff<10) then one motor at full power and other at half powe. This is done to move forward but with a power bias that steers the ROV closer towards final desired heading
ii. Both motors at full forward power if angle_diff is half of buffer angle (0<angle_diff<=5). The buffer angle can be increased or decreased; it is variably controlled through the software.