I'm just starting out with arduino, so please be kind!!!
I've started putting together a autopilot for a model boat, but i'm having a bit of trouble with the coding. I seem to be getting some really odd results from my servo, it stutters and jerks, the rudder position does not seem to relate very well to the course!!
There is loads more to add to the code (like adding an exponential function to the error to better hold the boat on course) but i really want to get this bit sorted first,
Here's the code:
#include <SoftwareServo.h>
#include <Wire.h>
SoftwareServo rudderservo; // create servo object to control a servo
int HMC6352Address = 0x42; // This is calculated in the setup() function
int slaveAddress;
int ledPin = 13;
boolean ledState = false;
byte headingData[2];
int i, headingValue;
int d, desiredValue;
int e, errorValue;
int a, amperrorValue;
int r, rudderangleValue;
void setup()
{
// Shift the device's documented slave address (0x42) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)
slaveAddress = HMC6352Address >> 1; // This results in 0x21 as the address to pass to TWI
Serial.begin(9600);
pinMode(ledPin, OUTPUT); // Set the LED pin as output
Wire.begin();
rudderservo.attach(3); // attaches the servo on pin 3 to the servo object
rudderservo.setMinimumPulse(650);
rudderservo.setMaximumPulse(2400);
}
void loop()
{
// Flash the LED on pin 13 just to show that something is happening
// Also serves as an indication that we're not "stuck" waiting for TWI data
desiredValue = 550; // Course to steer must not include the decimal point ie 25.5 = 255
// Send a "A" command to the HMC6352
// This requests the current heading data
Wire.beginTransmission(slaveAddress);
Wire.send("A"); // The "Get Data" command
Wire.endTransmission();
delay(10); // The HMC6352 needs at least a 70us (microsecond) delay
// after this command. Using 10ms just makes it safe
// Read the 2 heading bytes, MSB first
// The resulting 16bit word is the compass heading in 10th's of a degree
// For example: a heading of 1345 would be 134.5 degrees
Wire.requestFrom(slaveAddress, 2); // Request the 2 byte heading (MSB comes first)
i = 0;
while(Wire.available() && i < 2)
{
headingData[i] = Wire.receive();
i++;
}
headingValue = headingData[0]*256 + headingData[1]; // Put the MSB and LSB together
if (headingValue > desiredValue) //this should give you an angle in degrees
{ // between the desired heading and actual heading
errorValue = (headingValue - desiredValue);
}
else
{
errorValue = (desiredValue - headingValue);
}
if (errorValue > 1800) // if the error/agnle between heading and desired less 180degrees
{
errorValue = ((3600 - errorValue)*-1); // then 360° - error value, this should ensure the vessel
// is always turned the smallest angle to get back on course
}
rudderangleValue = errorValue ; // reads the value of the potentiometer (value between -1800 and 1800)
rudderangleValue = map(rudderangleValue, -180, 1800, 0, 179); // scale it to use it with the servo (value between 0 and 180)
rudderservo.write(rudderangleValue); // sets the servo position according to the scaled value
delay(15); // waits for the servo to get there
if (errorValue <= 10) {
digitalWrite(ledPin,HIGH); // will light an LED when on course
}
else
{
digitalWrite(ledPin,LOW);
}
SoftwareServo::refresh();
}
I have worked on many autopilots in my 30 years working in marine electronics and boatbuilding. looking at your code I would reccomend the following that should help tame your model autopilot.
rudder reference and rudder travel. rudder reference is really not important as long as you can keep the rudder from overtravelling and not turning in your case more than case 34 degrees. This gives you 17 degrees of travek from center to center. the servos should tell you where the rudder is so limiit the movement to slower. In mairne a slower rudder is better. what you really care about is what is the heading and what is the set or desired headiing value. thats it in gps ise rmb and rmc. if your not using gps just then a heading ouput is fine. The faster the boat goes the slower the rudder commands should be meaning the faster the boat goes the more responsive the rudder becomes so if its a fast speed model then you need to slow the rudder pwm down when the vesell hits plianing speed. make sense? if you make an h bridge with mossfts and a stepper motor just focus on the data coming from th eheading sensor and not course to steer but use rudder starboard or rudder port command and perhaps consider minimizing the pulses really close together of left and right in a way so that if the heading sensor reads 180 and you want to go 183 add right rudder of 2 second pwm then left rudder 500 microseconds and then right rudder 200 nanoseconds and left 200 nanoseconds get it? as long as the pilot knows heading it will run best looking at the heading value and your programming will have the most impact if you focus at keeping the heading stable sounds like your code the boat is left far off and then right like the nazi subhunter zig zag ? I know this is more theory than code but I have seen the worst code on the most expensive pilots run poorly and the best code on the cehapest hardware run best control law is expensive and think of the rudder as an aircraft wing:more smaller corrections are better than less major travel corrections? hope this helps and I would like to see more of your code and listen to your ideas? There is a good market for a out of the box product for model boats looking for autopilot control
Thanks @Rexmarlinspike for the reply, i'm keen to try and stick with servo setup rather than steppa motor to make the whole project less protype and more generic. As regards the rudder tracking, i am trying to setup the rudder much as you discussed, its just my codeing is not very good and the servo just doesnt do what i want it to do!!!!!
I figured if i put in an exponential sum in for the errorcode then map it down to a 120 degree servo turn (which will equate to approx 40 degree on the rudder stock) then it will essentially be like adjusting the gain, making the rudder more sensitive around the center point and less so at the endpoints. For now though i just want the error to translate to a servo position.
Ok, i've extracted the data from a serial print to a csv file then imported it into excel and it seems that my problem is centered around due north, as soon as the heading passes through due north the error changes from +ve to -ve , the error value is correct, but the -ve shouldnt change to +ve until it reaches the desired heading. Any help would be really appreciated, i know i should be able to figure it out, but i keep going over it and cant see it, its probably really obvious, i just cant see it.
as soon as the heading passes through due north the error changes from +ve to -ve , the error value is correct, but the -ve shouldnt change to +ve until it reaches the desired heading.
I don't know anything about your autopilot, but it sounds like you are connected to a compass that is doing what it is supposed to do, send a value that corrosponds to the current heading. I doubt that it knows anything about a "desired heading".
I am trying to do something similar with my model boat. How are you accounting for the waves or wind pushing the boat off course?
I am thinking of using a GPS and 3-axis gyro unit to help determine how much the boat is being pushed and how much it needs to adjust to get back on course.
I did have a think about adjusting for leeway when starting out and planning the project, but to do it successfully you need to know your speed through the water then you need to resolve the two vectors (desired course and speed with actual course and speed), then you get a error between the two if there is leeway, this will then give you a number of degrees to steer "offcourse" to achieve your desired course. See link below for a picture that probably describes things much better than i can.
But if you simply ignore leeway the gps course will change accordingly as you get closer to the waypoint inevitably getting you where you want to go. Not ideal and not how large ships operate, but on a model scale is probably overkill in my opinion.
As regards a 3-axis gyro, you may be able to get an approximation of leeway as regards to heel, but i wouldn't imagine it to be terribly accurate also it would not be able to compensate for tidal drift which would probably affect the model more than leeway (depending where in the world you are of course).
I am thinking about fitting a gyro to help me manage some movable ballast, so if you do come up with some code you would like to test let me know i'd be more than happy to give it a test run.
Hope some of this helps and do message me if i can help anymore.
