Where electronics enthusiasts find answers.
/*Code to make the Arduino turn the servo in the "target" direction and drive forward
based on the current orientation
Created on August 11th, 2012
*/
//Begining code is to initialize the compass and find the current orientation
#include <Wire.h> //includes the wire library for compass
#include <Servo.h> //includes the servo library to PWM the servo
int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41; //"A" in hex, A command is:
float GOAL = 90; //Taget of the compass (where to orient to)
Servo Steering; //defines the servo to be used to steer the car
Servo Motor; //defines the DC motor as a servo so it can be used with PWM
int headingValue;
int spd = 95; //speed of the DC motor
void setup(){
Steering.attach(10); //attaches the servo to pin 10
Motor.attach(9); //attach the DC motor to pin 9
Motor.write(spd); //drive the motor at speed--95
// "The Wire library uses 7 bit addresses throughout.
//If you have a datasheet or sample code that uses 8 bit address,
//you'll want to drop the low bit (i.e. shift the value one bit to the right),
//yielding an address between 0 and 127."
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1; // I know 0x42 is less than 127, but this is still required
Serial.begin(9600);
Wire.begin();
}
void loop(){
//"Get Data. Compensate and Calculate New Heading"
Wire.beginTransmission(HMC6352SlaveAddress);
Wire.write(HMC6352ReadAddress); // The "Get Data" command
Wire.endTransmission();
//time delays required by HMC6352 upon receipt of the command
//Get Data. Compensate and Calculate New Heading : 6ms
delay(6);
Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB
//"The heading output data will be the value in tenths of degrees
//from zero to 3599 and provided in binary format over the two bytes."
byte MSB = Wire.read();
byte LSB = Wire.read();
float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
float headingInt = headingSum / 10;
//Serial.print(headingInt);
//Serial.println(" degrees");
float OrientationDiff = GOAL - headingInt;
//defining OrientationDiff as the target minus the current orientaion
//Serial.print(OrientationDiff);
//Serial.println(" degrees");
if ((OrientationDiff >= -15) && (OrientationDiff <= 15)) //if difference ia greater or equal to -15 and less than or equal to 15
Steering.write(105); //go straight
else if ((OrientationDiff < -15) && (OrientationDiff > -215)) //if the difference is greater than 0 and less than 180
Steering.write(50); //turn left
else //anything else (if the difference is less than 0 and greater than 180
Steering.write(140); //turn right
}/* Code to make the Arduino turn the servo in the "target" direction
and drive forward based on the current orientation
Created on August 11th, 2012
*/
//Begining code is to initialize the compass and find the current orientation
#include <Wire.h> //includes the wire library for compass
#include <Servo.h> //includes the servo library to PWM the servo
// Prototypes
float NormalizeHalfCircle(float ang);
int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41; //"A" in hex, A command is:
float GOAL = 90.0; //Taget of the compass (where to orient to)
Servo Steering; //defines the servo to be used to steer the car
Servo Motor; //defines the DC motor as a servo so it can be used with PWM
int headingValue;
int spd = 95; //speed of the DC motor
void setup(void) {
Steering.attach(10); //attaches the servo to pin 10
Motor.attach(9); //attach the DC motor to pin 9
Motor.write(spd); //drive the motor at speed--95
// "The Wire library uses 7 bit addresses throughout.
//If you have a datasheet or sample code that uses 8 bit address,
//you'll want to drop the low bit (i.e. shift the value one bit to the right),
//yielding an address between 0 and 127."
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1; // I know 0x42 is less than 127, but this is still required
Serial.begin(9600);
Wire.begin();
}
void loop(void){
float OrientationDif;
//"Get Data. Compensate and Calculate New Heading"
Wire.beginTransmission(HMC6352SlaveAddress);
Wire.write(HMC6352ReadAddress); // The "Get Data" command
Wire.endTransmission();
//time delays required by HMC6352 upon receipt of the command
//Get Data. Compensate and Calculate New Heading : 6ms
delay(6);
Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB
//"The heading output data will be the value in tenths of degrees
//from zero to 3599 and provided in binary format over the two bytes."
byte MSB = Wire.read();
byte LSB = Wire.read();
float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
float headingInt = headingSum / 10;
//Serial.print(headingInt);
//Serial.println(" degrees");
// Defining OrientationDiff as the target minus the current orientaion
OrientationDiff = NormalizeHalfCircle(GOAL - headingInt);
// Compute correction.
#define GAIN 50.0*30.0 // 50 ms servo change for 30 degree error
#define OFFSET 105.0 // Servo neutral position
correction = OrientationDiff * GAIN + OFFSET;
//Serial.print(OrientationDiff);
//Serial.println(" degrees");
Steering.write(correction);
}
float NormalizeHalfCircle(float ang) {
if (ang > 180.0) ang -= 360.0
if (ang < -180.0) ang += 360.0
return (ang);
}
fll-freak wrote:untested codeCorrection should likely be a float and then cast to whatever the servo command wants.
Steering.write(correction);
}
float NormalizeHalfCircle(float ang) {
if (ang > 180.0) ang -= 360.0
if (ang < -180.0) ang += 360.0
return (ang);
}Steering.write(correction);
}
float NormalizeHalfCircle(float ang) {
if (ang > 180.0){ ang -= 360.0;}
if (ang < -180.0) {ang += 360.0;}
return (ang);
}