my boat are 1.30 meter hier has gas engine en servo en my probleem how to prograam it ?
i find this one but i need to reprograam en with xbee
#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(15);
}
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 = 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);*
}