How to make GPS and Compass calculation or programming for making the autonomous robot to drive straight in programming i am using GPS EM406a
and Compass HMC6352
How to make GPS and Compass calculation or programming for making the autonomous robot to drive straight in programming i am using GPS EM406a
and Compass HMC6352
The GPS will produce strings which include the latitude and longitude. You need to parse those strings to know your position and height.
IIRC the search word is NMEA
The Compass 6352 is a more difficult one to talk to. I have worked with one and it took several days to get reasonable readings from it. I started with a library for it but other issues came in between so it is not finished. This is the latest version I could find and it might be a starting point for you.
I recall that setting the different modes is not easy and at a certain moment the whole compass blocked. Took me long to get it running again, so the usage of the code below is at YOUR OWN risk. That means that all disclaimers apply, and that I won't help you fix the code or the blocking compass.
The query mode is the simplest.
//
// FILE: querymode.pde
// AUTHOR: Rob Tillaart
// VERSION: 0.1.00
// PURPOSE: test app HMC6352 library for Arduino
//
// Released to the public domain
//
#include <Wire.h>
#include <hmc6352.h>
hmc6352 Compass(0x21);
void setup()
{
Serial.begin(115200);
Serial.println("demo HMC6352: continuous mode");
Serial.println(HMC_LIB_VERSION);
Compass.setOperationalModus(CONT, 20, true); // 20 Hz
Serial.println(Compass.getOperationalModus());
Compass.direction(); // first time
}
void loop()
{
int x = Compass.qry();
Serial.print("DEGREE: ");
Serial.println(x);
// Serial.println(Compass.getOperationalModus(), BIN);
delay(50); // 20 Hz
}
and the .h file
//
// FILE: hmc6352.h
// AUTHOR: Rob Tillaart
// VERSION: 0.1.03
// PURPOSE: HMC6352 library for Arduino
//
// DETAILS: see cpp file
//
// Released to the public domain
//
//#ifndef hmc6352_h
//#define hmc6352_h
#include "Wprogram.h"
#define HMC_LIB_VERSION "0.1.01"
#define HMC_GET_DATA 0x41
#define HMC_WAKE 0x57
#define HMC_SLEEP 0x53
#define HMC_SAVE_OP_MODE 0x4C
#define HMC_CALLIBRATE_ON 0x43
#define HMC_CALLIBRATE_OFF 0x45
#define HMC_UPDATE_OFFSETS 0x4F
#define HMC_WRITE_RAM 0x47
#define HMC_READ_RAM 0x67
#define HMC_WRITE_EEPROM 0x77
#define HMC_READ_EEPROM 0x72
enum hmcMode { STANDBY=0, QUERY=1, CONT=2, ERROR};
class hmc6352
{
public:
hmc6352(uint8_t device);
// BASIC CALLS FOR STANDBY MODE
int getHeading(void); // just a merge of ask & read
int askHeading(void);
int readHeading(void);
int wakeUp(void);
int sleep(void);
// EXPERT CALLS
int factoryReset();
int setOperationalModus(hmcMode m, uint8_t freq, bool periodicReset);
int getOperationalModus();
int setOutputModus(uint8_t om);
int getOutputModus();
int callibrationOn(void);
int callibrationOff(void);
int setI2CAddress(uint8_t address);
int getI2CAddress();
int writeEEPROM(uint8_t address, uint8_t data);
int readEEPROM(uint8_t address);
int writeRAM(uint8_t address, uint8_t data);
int readRAM(uint8_t address);
// NOT TESTED / UNKNOWN
int setTimeDelay(uint8_t msec);
int getTimeDelay();
int setMeasurementSumming(uint8_t ms);
int getMeasurementSumming();
int saveOpMode(void);
int updateOffsets(void);
private:
int cmd(uint8_t c);
int readCmd(uint8_t c, uint8_t address);
int writeCmd(uint8_t c, uint8_t address, uint8_t data);
uint8_t _device;
};
//#endif
.cpp file
//
// FILE: hmc6352.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.1.02
// PURPOSE: HMC6352 library for Arduino
//
// HISTORY:
// 0.1.00 - 2011-04-07 initial version
// 0.1.01 - 2011-04-09 quite a complete redo
// 0.1.02 - 2011-04-12 added timing, fixed a bug
// 0.1.03 - 2011-04-13 fixed small things; added getHeading()
//
// Released to the public domain
//
#include <hmc6352.h>
#include <Wire.h>
#include "WProgram.h"
/* ERROR CODES ALL FUNCTIONS
//
// * twi_writeTo codes (== endTransmission commands)
// 0 .. OK
// -1 .. length to long for buffer
// -2 .. address send, NACK received
// -3 .. data send, NACK received
// -4 .. other twi error (lost bus arbitration, bus error, ..)
//
// * requestFrom
// -10 .. not enough values returned
//
// * function calls
// 0 .. OK
// -20 .. error param1
// -21 .. error param2
// -22 .. error param3
//
//
*/
hmc6352::hmc6352(uint8_t device)
{
Wire.begin();
_device = constrain(device, 0x10, 0xF6);
}
int hmc6352::getHeading()
{
int rv = askHeading();
if (rv != 0) return rv;
return readHeading();
}
// Ask the device to make a new reading
int hmc6352::askHeading()
{
int rv = cmd(HMC_GET_DATA);
if (rv != 0) return -rv; // problem with handshake
delay(6); // see datasheet, p8
return rv;
}
// read the last value from the
int hmc6352::readHeading()
{
int rv = Wire.requestFrom(_device, (uint8_t)2); // remove ambiguity
if (rv != 2) return -10;
rv = Wire.receive() * 256; // MSB
rv += Wire.receive(); // LSB
return rv;
}
// wake up from energy saving modus
int hmc6352::wakeUp()
{
int rv = cmd(HMC_WAKE);
delayMicroseconds(100);
return rv;
}
// go into energy saving modus
int hmc6352::sleep()
{
int rv = cmd(HMC_SLEEP);
delayMicroseconds(10);
return rv;
}
// values obtained from dump
int hmc6352::factoryReset()
{
writeRAM(0x74, 0x50); // is needed !!
writeCmd(HMC_WRITE_EEPROM, 0, 66);
writeCmd(HMC_WRITE_EEPROM, 1, 0);
writeCmd(HMC_WRITE_EEPROM, 2, 0);
writeCmd(HMC_WRITE_EEPROM, 3, 0);
writeCmd(HMC_WRITE_EEPROM, 4, 0);
writeCmd(HMC_WRITE_EEPROM, 5, 1);
writeCmd(HMC_WRITE_EEPROM, 6, 4);
writeCmd(HMC_WRITE_EEPROM, 7, 6);
writeCmd(HMC_WRITE_EEPROM, 8, 0x50);
cmd(HMC_SAVE_OP_MODE);
delayMicroseconds(125);
return 0;
}
// HANDLE WITH CARE - RESTART NECESSARY
// Returns Operational Mode Config Byte
int hmc6352::setOperationalModus(hmcMode m, uint8_t freq, bool periodicReset)
{
byte omcb = 0; // Operational Mode Control Byte
switch(freq)
{
case 1: break;
case 5: omcb |= 0x20; break;
case 10: omcb |= 0x40; break;
case 20: omcb |= 0x60; break;
default: return -21;
}
if (periodicReset) omcb |= 0x10;
switch(m)
{
case STANDBY: break; // omcb |= 0x00;
case QUERY: omcb |= 0x01; break;
case CONT: omcb |= 0x02; break;
default: return -20;
}
writeCmd(HMC_WRITE_RAM, 0x74, omcb);
cmd(HMC_SAVE_OP_MODE);
delayMicroseconds(125);
return omcb;
}
// read the Operational Modus as byte from EEPROM
// TODO: split into 3 items
//
int hmc6352::getOperationalModus()
{
// datasheet state that at startup the OM is copied from EEPROM
// and that after writing to RAM a reboot is needed to enable new settings
// my interpretation ==> EEPROM is leading
return readCmd(HMC_READ_RAM, 0x74);
//return readCmd(HMC_READ_EEPROM, 0x08);
}
// Switch between normal heading and raw modes
// default = 0 ==> normal headings
// Note: after a reboot the output modus will be 0 again.
int hmc6352::setOutputModus(uint8_t om)
{
if (om > 4) return -20;
return writeCmd(HMC_WRITE_RAM, 0x4E, om);
}
// Read the output modus from RAM
int hmc6352::getOutputModus()
{
return readCmd(HMC_READ_RAM, 0x4E);
}
// NOT TESTED
int hmc6352::callibrationOn()
{
int rv = cmd(HMC_CALLIBRATE_ON);
delayMicroseconds(10);
return rv;
}
// NOT TESTED
int hmc6352::callibrationOff()
{
int rv = cmd(HMC_CALLIBRATE_OFF);
delay(15);
return rv;
}
// NOT TESTED
int hmc6352::setI2CAddress(uint8_t address)
{
if (address < 0x10 || address > 0xF6 ) return -20;
return writeCmd(HMC_WRITE_EEPROM, 0, address);
}
// returns current I2C address
int hmc6352::getI2CAddress()
{
return readCmd(HMC_READ_EEPROM, 0);
}
// NOT TESTED
// meaning time delay unknown
// therefore removed from lib for now
int hmc6352::setTimeDelay(uint8_t msec)
{
return writeCmd(HMC_WRITE_EEPROM, 5, msec);
}
int hmc6352::getTimeDelay()
{
return readCmd(HMC_READ_EEPROM, 5);
}
// NOT TESTED
// meaning measurement summing unknown
// therefore removed from lib for now
int hmc6352::setMeasurementSumming(uint8_t ms)
{
if (ms > 16 ) ms = 16;
return writeCmd(HMC_WRITE_EEPROM, 6, ms);
}
int hmc6352::getMeasurementSumming()
{
return readCmd(HMC_READ_EEPROM, 6);
}
// Makes only sense in setOperationalModus()
// therefore removed from lib for now
int hmc6352::saveOpMode()
{
int rv = cmd(HMC_SAVE_OP_MODE);
delayMicroseconds(125);
return rv;
}
// NOT TESTED
// meaning UpdateOffsets unknown
// therefore removed from lib for now
int hmc6352::updateOffsets()
{
int rv = cmd(HMC_UPDATE_OFFSETS);
delay(6);
return rv;
}
// idem
// use at own risk ...
int hmc6352::writeEEPROM(uint8_t address, uint8_t data)
{
return writeCmd(HMC_WRITE_EEPROM, address, data);
}
// idem
int hmc6352::readEEPROM(uint8_t address)
{
return readCmd(HMC_READ_EEPROM, address);
}
// idem
// Most RAM locations have an unknown meaning
// use at own risk ...
int hmc6352::writeRAM(uint8_t address, uint8_t data)
{
return writeCmd(HMC_WRITE_RAM, address, data);
}
// idem
int hmc6352::readRAM(uint8_t address)
{
return readCmd(HMC_READ_RAM, address);
}
/* PRIVATE FUNCTIONS
mostly to remove redundancy in functions above
*/
int hmc6352::cmd(uint8_t c)
{
Wire.beginTransmission(_device);
Wire.send(c);
int rv = Wire.endTransmission();
delay(10);
return rv;
}
int hmc6352::readCmd(uint8_t c, uint8_t address)
{
Wire.beginTransmission(_device);
Wire.send(c);
Wire.send(address);
int rv = Wire.endTransmission();
if (rv != 0) return -rv;
delayMicroseconds(70);
rv = Wire.requestFrom(_device, (uint8_t)1);
if (rv != 1) return -10;
rv = Wire.receive();
return rv;
}
int hmc6352::writeCmd(uint8_t c, uint8_t address, uint8_t data)
{
Wire.beginTransmission(_device);
Wire.send(c);
Wire.send(address);
Wire.send(data);
int rv = Wire.endTransmission();
delayMicroseconds(70);
return rv;
}
My Robot works fine on Compass heading Problem is that i want to utilize the GPS heading so that it can move on Accurate direction
Code?
its my simple request kindly tell me the place where i am wrong
i am facing Problem in calculating the Heading of the GPS and then combining it with my compass so that my Robot moves straight towards the destination
#include <math.h>
#include <nmea.h>
#include <Wire.h>
#include <LiquidCrystal.h>
LiquidCrystal myLCD(42, 40, 38, 36, 34, 32);
float lat;
float lon;
NMEA myGPS(GPRMC);
float dest_latitude = 33.693792;
float dest_longitude = 73.053170;
int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41;
const int potPin = A8;
const int leftMotor = 27; //PWM pin to the L293
const int rightMotor = 26; //PWM pin to the L293
int motorA1=22;
int motorA2=23;
char Dir;
bool isLeft() {
return (analogRead(potPin)/4) <85;
}
bool isLeftOfCenter() {
return (analogRead(potPin)/4) < 115;
}
bool isRightOfCenter() {
return (analogRead(potPin)/4) > 147;
}
bool isRight() {
return (analogRead(potPin)/4) > 157;
}
int beeperpin=44;
void setup() {
Serial.begin(9600);
Serial1.begin(4800);
myLCD.begin(16,2);
Wire.begin();
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;
pinMode(leftMotor,OUTPUT);
pinMode(rightMotor,OUTPUT);
pinMode(motorA1,OUTPUT);
pinMode(motorA2,OUTPUT);
myLCD.print("GPS BASED");
delay(3000);
myLCD.clear();
myLCD.home();
pinMode(13, OUTPUT);
pinMode(8, OUTPUT);
pinMode(6, OUTPUT);
}
void loop() {
if (Serial1.available() > 0 ) {
if (myGPS.decode(Serial1.read())) {
if (myGPS.gprmc_status() == 'A') {
lat=myGPS.gprmc_latitude();
lon = myGPS.gprmc_longitude();
Serial.print("D:");
float d=(round(myGPS.gprmc_distance_to(dest_latitude, dest_longitude, MTR)));
Serial.print(d);
Serial.print("m");
Serial.print('\n');
myLCD.setCursor(5, 0);
myLCD.print(d);
myLCD.print("m");
if (d < 18){
myLCD.clear();
myLCD.home();
myLCD.setCursor(5,0);
myLCD.print("Destinat");
digitalWrite(13, HIGH);
digitalWrite(beeperpin, HIGH);
carbreak();
steering('C');
}
else if (d >= 18){
digitalWrite(13, LOW);
myLCD.clear();
myLCD.home();
myLCD.setCursor(5, 0);
myLCD.print(d);
myLCD.print("m");
float currentdir = compassvalue();
//myLCD.setCursor(4,2);
//myLCD.print(currentdir);
if (currentdir < 358 && currentdir > 200)
{
myLCD.setCursor(4,1);
Serial.print ('<');
// digitalWrite(8, HIGH);
// digitalWrite(6, LOW);
//myLCD.clear();
myLCD.print ("LEFT");
steering('L');
delay(500);
motorForward();
}
if (currentdir < 160 && currentdir > 2)
{
Serial.print ('>');
myLCD.setCursor(4,1);
//digitalWrite(6, HIGH);
//digitalWrite(8, LOW);
//myLCD.clear();
myLCD.print ("RIGHT");
steering('R');
delay(500);
motorForward();
}
if (currentdir > 160 && currentdir < 200)
{
Serial.print ('=');
myLCD.setCursor(4,1);
//myLCD.clear();
myLCD.print("center");
//digitalWrite(6, LOW);
//digitalWrite(8, LOW);
steering('C');
delay(300);
motorForward();
}
}
}
}
}
}
// if (currentdir < 20 && currentdir > 2)
// {
// Serial.print ('=');
// myLCD.setCursor(1,1);
// myLCD.clear();
// myLCD.print("center");
// //digitalWrite(6, LOW);
// //digitalWrite(8, LOW);
// steering('C');
// }
int calc_bearing(float flat1, float flon1, float flat2, float flon2)
{
float calc;
float bear_calc;
float x = 69.1 * (flat2 - flat1);
float y = 69.1 * (flon2 - flon1) * cos(flat1/57.3);
calc=atan2(y,x);
bear_calc= degrees(calc);
if(bear_calc<=1){
bear_calc=360+bear_calc;
}
return bear_calc;
}
int compassvalue(){
//"Get Data. Compensate and Calculate New Heading"
Wire.beginTransmission(HMC6352SlaveAddress);
Wire.send(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.receive();
byte LSB = Wire.receive();
float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
float headingInt = headingSum / 10;
//Serial.print(headingInt);
//Serial.println(" degrees");
return headingInt;
}
void motorStop() {
digitalWrite(leftMotor, LOW);
digitalWrite(rightMotor, LOW);
}
void motorLeft() {
digitalWrite(leftMotor, HIGH);
}
void motorRight() {
digitalWrite(rightMotor, HIGH);
}
//back wheels
void motorForward()
{
digitalWrite(motorA1,HIGH);
digitalWrite(motorA2,LOW);
}
void motorBackward()
{
digitalWrite(motorA1,LOW);
digitalWrite(motorA2,HIGH);
}
void carbreak()
{
digitalWrite(motorA1,LOW);
digitalWrite(motorA2,LOW);
}
void steering(char Dir)
{
//if (Serial.available() == 0)
//return;
//Dir=toupper(Serial.read());
switch (Dir)
{
case 'L':
//Serial.println("New command: L");
if (isLeft())
{
}//Serial.println("Already Left");
else
{
//Serial.println("Heading Left");
motorLeft();
while (!isLeft()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Left");
}
break;
case 'C':
//Serial.println("New command: C");
if (isRightOfCenter())
{
//Serial.println("Heading Left toward Center");
motorLeft();
while (isRightOfCenter()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Centered");
}
else
if (isLeftOfCenter())
{
//Serial.println("Heading Right toward Center");
motorRight();
while (isLeftOfCenter()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Centered");
}
else
{
}
//Serial.println("Already Centered");
break;
case 'R':
//Serial.println("New command: R");
if (isRight())
{
}//Serial.println("Already Right");
else
{
//Serial.println("Heading Right");
motorRight();
while (!isRight()) /* JUST WAITING */;
motorStop();
//Serial.println("Is now Right");
}
break;
} // end of switch on direction
}
You seem to think we have some magic debug wand and mystical psychic powers that enable us to see what your debug output looks like, and how your system behaves.
For the very last time - we do not.
That is where you are going wrong.
cutebuddy6:
My Robot works fine on Compass heading Problem is that i want to utilize the GPS heading so that it can move on Accurate direction
I don't understand what this means, please clarify: what do you mean by "GPS heading"?
by GPS heading i mean to say the Bearings from GPS
cutebuddy6:
by GPS heading i mean to say the Bearings from GPS
I understand how you get heading information from a compass, but I don't understand how you get it from a GPS receiver - unless the receiver also has a compass built in which you are using to get the heading. Can you clarify exactly what you mean by GPS heading?
i mean to say the calculated bearing through lat\long
cutebuddy6:
i mean to say the calculated bearing through lat\long
That makes more sense. So you use GPS to find your current lat/long, and have somehow got the lat/long of the place you want to go towards. Is that the idea?
In order to get there you really need to know which way you're pointing as well as where you are. The algorithm would be something like:
Find current position from GPS.
Calculate desired direction of travel to the waypoint.
Find orientation of vehicle from compass.
Calculate angular offset between vehicle orientation and desired direction of travel.
Calculate steering position to steer towards the desired orientation. Steering position should be based on the angular offset so that the amount of steering reduces as the angular offset reduces, until eventually the vehicle is travelling in a straight line towards the waypoint. If the vehicle is going to be travelling at high speed, you should scale the steering angle according to the speed to prevent the system from being oversensitive at high speed.