Hi all!
I found a old post about arduino autopilot ( https://forum.sparkfun.com/viewtopic.php?f=14&t=34365) and in this code he has use HMC6343 compass module. I tried to make one like that but my problem is that, I'm using HMC6352 module and unfortunately, i can't receive data from it. I changed the address to 42 but, nothing!
Any help me on it? Thank you!
#include <Wire.h>
#include <Serial.h>
#include <Servo.h>
#define compassAddress 0x42 >> 1
Servo rudder; //Rudder servo object
const float minRudder = - 40.0;
const float maxRudder = 25.0;
const float centerRudder = 90.0;
const float offsetRudder = 9.5; //Deg to correct prop torque
const unsigned int smallWiggle = 10;
const unsigned long testDelay = 1000;
const unsigned long loopDelay = 1000;
byte compassReadingBytes[2]; //Heading read bytes from compass
unsigned int rudderCommand = 90; //Command for rudder servo
float compassReading = 0.0; //Reading from compass
float desiredHeading = 0.0; //Heading to be followed
float errorDeg = 0.0; //Error btw course and desired course
float Gain = 1.5; //Gain for proportional loop
float rudderPosition = 0.0; //Desired position of rudder
unsigned int loopCount = 0; //Running loop count
unsigned long initTime = 0; //Time when compass was initialized
void setup() {
Serial.begin(38400); //Start the serial port for testing
Serial.println("Initializing the system");
Wire.begin(); //Start i2c
// put any needed compass init code below this line
// Wait for the compass to initialise
initTime = millis();
rudder.attach(9); //Rudder servo on pin 9
Serial.println("Doing a rudder servo test");
//Command hard over
rudderCommand = word(minRudder + 90);
rudder.write(rudderCommand);
delay(testDelay);
//Command hard over the other way
rudderCommand = word(maxRudder + 90);
rudder.write(rudderCommand);
delay(testDelay);
//Command back to center
rudderCommand = 90;
rudder.write(rudderCommand);
delay(testDelay);
//check that enough time has passed for compass to be ready
while(millis() - initTime < 500 ){
delay(10);
}
//get an average of 10 readings to set desired heading
for(int i = 0; i < 10; i++){
readCompass(); //Get heading from compass
desiredHeading += compassReading/10.0;
delay(210);
}
Serial.print("Desired heading is ");
Serial.println(desiredHeading);
Serial.println(" ");
//Now do a quick rudder wiggle to let the operator
//know that the tiki is ready for launch
//Command -10 deg
rudderCommand = 90 - smallWiggle;
rudder.write(rudderCommand);
delay(testDelay/4);
//Command +10 deg
rudderCommand = 90 + smallWiggle;
rudder.write(rudderCommand);
delay(testDelay/2);
//Command back to center
rudderCommand = 90;
rudder.write(rudderCommand);
delay(testDelay/4);
}
void loop(){
loopCount++;
readCompass();
// compute how many deg offcourse tiki is
errorDeg = compassReading - desiredHeading;
if(errorDeg >= 180.0){
errorDeg = errorDeg - 360.0;
}
if(errorDeg <= -180.0){
errorDeg = errorDeg + 360.0;
}
// compute rudder position needed to correct error
rudderPosition = constrain((Gain * errorDeg), minRudder, maxRudder);
// translate that position into a servo command
//rudderCommand = 90 - word(rudderPosition);
rudderCommand = word(rudderPosition + centerRudder + offsetRudder);
rudder.write(rudderCommand);
Serial.print("Loop count is ");
Serial.println(loopCount);
Serial.print("Measured heading is ");
Serial.println(compassReading);
Serial.print("Rudder position is ");
Serial.println(rudderPosition);
Serial.print("Servo command is ");
Serial.println(rudderCommand);
Serial.println(" ");
delay(loopDelay);
}
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++) {
compassReadingBytes[i] = Wire.read();
}
}
compassReading = ((int)compassReadingBytes[0]<<8) | ((int)compassReadingBytes[1]); // heading MSB and LSB
compassReading = compassReading * 0.1; //Translate heading to degress
if(compassReading == 360.0){
compassReading = 0.0;
}
}