SparkFun Forums

[boatbandit2]

Hi i have been working on this project for some time now i am using a Ardunino R3 and a honey well HMC6343 digital compass. I did have a post here before but had little help from follow members for over 120 days so i deleted it ,So I hope someone will help now
I would like to add GPS to this set up as the compass is not keeping the Kontiki on course.I am looking at buying the Adafruit Ultimate GPS Breakout .It will be under water as it goes under breaking waves.
So i was wondering if anyone could help me to add a gps to my setup and how i can add it to my code that i have it is a very basic system as when i point the Kontiki in the direction of travel and switch on the Arduino the compass locks onto a bearing and adjusts the servo to keep the craft on its bearing.One other problem i am having is i need to adjust the servo from its center point to allow for propeller torque say 1 to 2 degrees to the right but i am unsure how to add this to my code.I have added my code below

Code: Select all

#include <Wire.h>
#include <Serial.h>
#include <Servo.h>

#define compassAddress 0x32 >> 1
#define adjustment 15 //Degress to adjust rudder
#define readDelay 2500  //Time between compass reads

Servo rudder;  //Rudder servo object
int rudderAngle = 0;  //current angle of the rudder servo
int rudderPosition = 0;  //New position for rudder

unsigned int compassHeading = 0;
unsigned int initialHeading = 0;
unsigned int heading = 0;

int offset = 0;
int tempOffset = 0;
int headingShift = 0;

byte compassHeadingBytes[2];  //Heading read bytes from compass
int i = 0;

void setup() {
  Serial.begin(115200);  //Start the serial port for testing

  Wire.begin();  //Start i2c
  
  rudder.attach(9);  //Rudder servo on pin 9
  
  while( millis() < 500 ) {  //Wait for the compass to initialise
    delay(10);
  }
  
  rudderAngle = rudder.read(); //Get initial angle of rudder servo
  readCompass();  //Get heading from compass

  setPosition();  //translate into direction segment
  setOffset();  //Set offset to correct heading
  adjustPosition();  // Adjust heading to new position

  initialHeading = heading;  //Save heading as target direction
}

void loop() {
  
  readCompass();
  setPosition();
  adjustPosition();
  setServo();

  delay(readDelay);

}

void readCompass() { 
  //Instruct compass to read echoes 
  Wire.beginTransmission(compassAddress);  // transmit to device
                           // the address specified in the datasheet is 66 (0x42) 
                           // but i2c adressing uses the high 7 bits so it's 33 
  Wire.write(0x50);         // Send a "Post Heading Data" (0x50) command to the HMC6343  
  Wire.endTransmission();  // stop transmitting 
 
  //Wait for readings
  delay(2);               // datasheet suggests at least 1 ms 
  
  //Request heading reading from compass 
  Wire.requestFrom(compassAddress, 2);  // request 2 bytes from slave device #33 
 
  //Receive heading reading from compass 
  if(2 <= Wire.available())     // if 2 bytes were received 
  {
    for(int i = 0; i<2; i++) {
      compassHeadingBytes[i] = Wire.read();
    }
  }
  
  compassHeading = ((int)compassHeadingBytes[0]<<8) | ((int)compassHeadingBytes[1]);  // heading MSB and LSB
  compassHeading = compassHeading * 0.1;  //Translate heading to degress

}
// Translate Compass reading to directional segment
// LSB = N, MSB = NNE.
void setPosition(){
  if ( compassHeading >= 349 || compassHeading <= 11 ) {
    heading = 1;
  }
  else if ( compassHeading >= 12 && compassHeading <= 33 ) {
    heading = 2;
  }
  else if ( compassHeading >= 34 && compassHeading <= 56 ) {
    heading = 4;
  }
  else if ( compassHeading >= 57 && compassHeading <= 78 ) {
    heading = 8;
  }
  else if ( compassHeading >= 79 && compassHeading <= 101 ) {
    heading = 16;
  }
  else if ( compassHeading >= 102 && compassHeading <= 123 ) {
    heading = 32;
  }
  else if ( compassHeading >= 124 && compassHeading <= 146 ) {
    heading = 64;
  }
  else if ( compassHeading >= 147 && compassHeading <= 168 ) {
    heading = 128;
  }
  else if ( compassHeading >= 169 && compassHeading <= 192 ) {
    heading = 256;
  }
  else if ( compassHeading >= 193 && compassHeading <= 213 ) {
    heading = 512;
  }
  else if ( compassHeading >= 214 && compassHeading <= 236 ) {
    heading = 1024;
  }
  else if ( compassHeading >= 237 && compassHeading <= 258 ) {
    heading = 2048;
  }
  else if ( compassHeading >= 259 && compassHeading <= 281 ) {
    heading = 4096;
  }
  else if ( compassHeading >= 282 && compassHeading <= 303 ) {
    heading = 8192;
  }
  else if ( compassHeading >= 304 && compassHeading <= 326 ) {
    heading = 16384;
  }
  else if ( compassHeading >= 327 && compassHeading <= 348 ) {
    heading = 32768;
  }
}

//Ofset based on segment seting, so initial heading is 256.
void setOffset() {
  switch ( heading ) {
    case 1:
      offset = 1;
      break;
    case 2:
      offset = 2;
      break;
    case 4:
      offset = 3;
      break;
    case 8:
      offset = 4;
      break;
    case 16:
      offset = 5;
      break;
    case 32:
      offset = 6;
      break;
    case 64:
      offset = 7;
      break;
    case 128:
      offset = 8;
      break;
    case 256:
      offset = 9;
      break;
    case 512:
      offset = 10;
      break;
    case 1024:
      offset = 11;
      break;
    case 2048:
      offset = 12;
      break;
    case 4096:
      offset = 13;
      break;
    case 8192:
      offset = 14;
      break;
    case 16384:
      offset = 15;
      break;
    case 32768:
      offset = 16;
      break;
  }
  offset = 8 - offset;
}

// Adjust heading by offset
void adjustPosition(){

  unsigned int headingTemp = heading;

  if ( offset >= 0 ) {
    tempOffset = offset;
    while ( tempOffset > 0 ) {
      headingShift = headingTemp << 1;
      headingTemp = headingShift;
      if ( headingTemp == 0 ) {
        headingTemp = 1;
      }
      tempOffset = tempOffset - 1;
    }
  }
  else {
    switch (offset) {
     case -1:
       tempOffset = 1;
       break;
     case -2:
       tempOffset = 2;
       break;
     case -3:
       tempOffset = 3;
       break;
     case -4:
       tempOffset = 4;
       break;
     case -5:
       tempOffset = 5;
       break;
     case -6:
       tempOffset = 6;
       break;
     case -7:
       tempOffset = 7;
       break;
     case -8:
       tempOffset = 8;
       break;
    }
    while ( tempOffset > 0 ) {
      headingShift = headingTemp >> 1;
      headingTemp = headingShift;
      if ( headingTemp == 0 ) {
        headingTemp = 32768;
      }
      tempOffset = tempOffset - 1;
    }
  }
  heading = headingTemp;
}

void setServo() {
  unsigned int i,j;
  
  rudderPosition = rudderAngle;
  i = heading;
  if ( heading < 128 ) {
    while ( i < 128){
      rudderPosition = rudderPosition - adjustment;
      j = i << 1;
      i = j;  
    }
  }
  if ( heading > 128 ) {
    while ( i > 128 ) {
      rudderPosition = rudderPosition + adjustment;
      j = i >> 1;
      i = j;
    }
  }
  rudder.write(rudderPosition);
}

[jremington]

The HMC6343 is a tilt-compensated compass and should provide all the info that you need to keep a constant bearing. So, why doesn't it work? Have you checked whether the motor and/or servo affect the compass reading when they are running?

Assuming that stray magnetic fields are not the problem, I can't make much sense of your code -- it looks way, way more complicated than it has to be to maintain a bearing. A PID loop, which compares the present heading in degrees to the desired heading in degrees and takes corrective action, should be able to accomplish the task in just 5-10 lines of code.

In your shoes, I would try to figure out what is going wrong with the compass approach before attempting to add a GPS.

[boatbandit2]

Hi i have put the compass in the rear of the kontiki so it is be about 1 meter away from the motor and the motor has a alum body as for the code do you have any examples that i could try i am new to this i probably have gone over board,

[jremington]

There are thousands of web pages that describe PID control, which is what you need for this application. Do some research! There are several threads on this forum that discuss boat-type autopilot issues, and then there is the entire ArduPilot project, which has lots of info for beginners. The ArduRover is closest to what you need. http://www.diydrones.com/notes/ArduPilot

Until you describe your hardware in reasonable detail, no one will be able to give you a useful code example.

Finally, you ABSOLUTELY MUST CHECK whether the compass reading changes AT ALL if the motor and/or servo is running. If it does change, your system will never work properly.

[Mee_n_Mac]

OK, first things first. I recall some kontiki fishing project a while ago, thought it was kinda kewl as I'd never heard of a kontiki before. So when you say it's not keeping on course, what do you think is happening ? Is the kontiki heading in the proper direction (for the most part) and it's the rips and/or tides and/or drag from the fishing line that are sweeping it laterally offcourse ? Or is it not going North when you tell it to go North ? Does it work in calm water but not in waves ? With or w/o the line attached ? Some better idea of what's going wrong will aid us in coming up with a better solution. GPS may be that solution but it adds another level of complexity that won't help if it's the steering servo system that's ineffective.

ps : use the code tags to better display any code you post
(click on to enlarge)

CodeButtonTags.jpg

[boatbandit2]

Well i have set it out six times now and it will drive to the right with the line attach with about 3 kg of weight, hooks sinkers then about 100 meters out it will do a full circle then it will go back onto its heading still going to the right about 2 to 3 degrees. The wind has always been onshore and the beach i am trying it in has very little rip,i have adjusted the rudder to steer the left about 2 degrees to allow for prop torque.I have tried it with no weight same thing happens.

I am using a Arduino Uno R3 and a Honey well HMC6343 digatil compass which is controlling a Savox SA-1283SG Super Torque Steel Gear Digital Servo-savsa1283sg ,Speed(@6.0V sec/60): .13,Torque(@6.0V oz-in): 416.6,Bearing: 2BB
Case: Full Aluminum

[jremington]

Here is a very lengthy and interesting discussion on DIY boat autopilots:
http://www.boatdesign.net/forums/onboar ... 30075.html

[Mee_n_Mac]

Can you put this part in English ?

"it will go back onto its heading still going to the driving to the right about 2 to 3 degrees"

So it consistently goes out ~100 m, following the desired course/compass setting. No 2-3 deg error ?

Then it does a 360 deg circle. That's damn odd. My first inclination is that the line drag is somehow causing this. But I won't dismiss something odd in the code. What compass heading is the 'tiki set to follow ? Is it anywhere close to 0 deg magnetic ?

Then is goes back on course ? Or goes almost back on course but off by 2-3 deg to the right ?? Or ???

It does all of the above after you've adjusted the rudder 2 deg to the left ? FWIW you shouldn't have to adjust anything. Other than to allow the rudder to have the same effective range, turning to port or starboard, the control system should command the rudder to whatever angle is needed to counter the prop-walking.

[boatbandit2]

sorry i have my 6 year old at my feet, it always drives off course by about 2 to 3 degrees to the right and it goes almost back on course but off by about 2-3deg to the right it always steers right about 2-3 deg,

cheers

[boatbandit2]

Thank you jremington and Mee_n_Mac for your input.

[Mee_n_Mac]

While you think it's not "drift", I'm not 100% sure that can be ruled out. But the looping is very odd and perhaps a telltale of something else. Have you looked at the compass readings with and w/o the motor running (as suggested by jr) ? If you can see the compass readings, does the rudder deviate when the motor comes on ? Check by holding the tiki in your hands on dry land.

Have you verified the steering works equally well on all points of the compass ? That is aim the 'tiki due North and have it memorize that heading. Then, again on dry land, aim the tiki some 10+ deg to the right and then to the left. Verify the rudder moves equally in each direction so as to compensate for the induced heading error. Repeat this at a number of points of the compass, setting the initial heading at 45, 90, 135, 180, 225, 270 and 315 deg and doing the wiggle test.

Does the rudder swing the same amount, "left" and "right" for equal heading errors (neglecting the 2-3 deg offset you've dialed in) ?

Have you run the 'tiki out w/o a line attached ? I know you'd have to row out to get it. Perhaps with a very very light retrieval line then.

[Mee_n_Mac]

OK, I've now looked at your code 3x and for the life of me I can't tell how it works ... I'm assuming it does. Maybe tomorrow after I've had some sleep it'll make more sense. What I can say is that there's a much much much simpler and better way to do what I think you're trying to do, w/o even getting into PID loops (start with a simple P control loop). You can then advance as desired or needed from there. But I do have a few questions ...
1) How do you set the heading to be held ? I take it there's no switch and so I deduce that you must aim the 'tiki and then either turn it on or reset it somehow. The 'tiki then memorizes whatever heading it reads during setup(). Am I correct ?
2) Why do you read the rudder position in setup() ? Do you use this for anything ? Why ?

3) What commanded rudder position would correspond to straight ahead, ideally ? Assume no prop-walk. What are the limits to your servo motion and commands ? +/- 90 deg ?? +/- 45 deg ?? What's the mechanical relationship between the servo angle and the rudder angle ? That is, if you command the servo to move 5 deg, how much does the rudder move ?

[boatbandit2]

Hi Mee_n_ Mac
1) question yes you are correct i aim the kontiki then turn on a toogle switch and the compass memorizes what ever heading reads.ie 45,65,70 deg
2) That is incase it is not at its center postion,
3) The rudder moves 15 degs at the monent with the #define adjustment 15 //Degress to adjust rudder.
The rudder moves equal to the rudder movements the arms are all parallel so if the servo moves 10 deg the rudder moves 10 deg give or take 1 deg.
I have checked and the motor is not affecting the compass i did notice if it leans to one side it adjust the servo ie rudder.

I think my code is just crap mate i have tried and failed i am only a Carpentered mate i think it took some pros to show me that.

Cheers

Paul

[Mee_n_Mac]

OK, I modified your code so I could run it here w/o a compass. Instead I just enter fake compass readings via the keyboard and serial monitor. I see how the code works now. And I think I know what's going on. I don't know how to sugar coat this but my 1'st GUESS at your problem is that the code is commanding the rudder to go the wrong way. That is when the tiki veers off to starboard (as in your diagram), the code should command the rudder to swing in a direction that makes the tiki go back towards port. Instead the code is commanding the tiki to go even more to starboard. What happens then is like the greenhouse effect. The more the code (wrongly) commands the rudder to move, the more the tiki continues to turn in the wrong direction, indeed faster and faster. The result is the loop you see. You get out of the looping behavior because at some point in the loop your compass reading almost equals your initial heading (maybe) and the rudder command goes to zero. The tiki then runs straight (more or less) and on course until it again drifts (slowly) enough off course to get past your 15 deg threshold (or 12 deg with the offset you have). Once past that threshold it'll loop again.

You can verify this by looking at the direction the rudder moves for a heading error. On land hold the tiki and start as normal. Let it memorize the heading to hold. Then turn it 20 deg to starboard. Watch the rudder direction. Does that look correct to you ? I'm assuming if you fish, you boat and so will know from experience what looks right and wrong. If not, take a picture and post it here of that test. Then turn the tiki back to "on course" and watch the rudder go back to ~ zero (straight ahead). Now turn the tiki to 20 deg to port and see if the rudder moves at all, and if so in what direction. I think the rudder never goes in the opposite direction ! In the meanwhile I can probably make a diagram to show what's right and wrong. (EDIT : actually I now think the rudder control works as in my post below)

Right now I've yet to understand what the setServo() function is trying to do. I can say I don't think it does what you intended it to do. In fact I can see it ever turning the rudder to make the tiki turn to port. Even worse I don't see the code ever using the initial heading at all. I don't see that there's a control loop that determines the heading error and commands a rudder position based on that error. Instead the code seems to command a rudder position based on the compass reading. As long as the tiki is headed due North (= 0 deg compass reading) (EDIT : nope, it's as below, ESE-SE) it commands no rudder movement. And deviation from due North (ESE-SE) makes a rudder movement that, apparently, turns the tiki ever and ever harder to starboard. Have you always launched this tiki aimed initially due North (or pretty close to it) ? What rudder do you see if you have it aimed due East when you turn it on ? I'd hope that so long as you aimed due East when it was turned on and you don't move it, the rudder should be straight ahead, except for the prop-walk offset you put in. But I doubt that's what will happen if you do this test. Do it and prove me wrong.

[Mee_n_Mac]

OK I looked and played some more and I think the rudder is commanded dead ahead only when the compass reading is between 125 and 145 deg (give or take). Less than 125 gets a "left" rudder motion and more than 145 gets the opposite direction. If you launch with a compass heading btw 125 and 145 (ESE to SE) the tiki will go straight (more or less) and slightly drift off course as time goes by. If the drift goes to starboard then you'll get a loop, as shown in your diagram above, until the tiki is headed back to 125 deg. If the drift goes to port, it'll loop in the opposite direction until the compass reading gets to 145 deg.