Hi,
I am doing a robot that has to follow the coordinates that I am sending from a smartphone to the arduino via bluetooth. I want to make sure first that the robot is facing the right bearing but I am unable to do that. I do not know if it is the GPS accuracy or if the code is anywhere wrong... Could you please help me with it... The robor is receiving the coordinates both from the GPS and bluetooth (latitude and longitude) but it does not facing the right direction...
#include "TinyGPS.h"
#include <LiquidCrystal.h>
#include <SparkFun_MAG3110.h>
String input = "";
String lat, lon;
float latitude, longitude, flat, flon, d, bearing, heading;
int commaPosition;
char c;
TinyGPS gps;
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
MAG3110 mag = MAG3110(); //Instantiate MAG3110
#define CMD (byte)0x00
#define SET_MODE 0x34
#define SET_ACCEL 0x33
#define SET_SPEED1 0x31
#define SET_SPEED2 0x32
void setup() {
Serial.begin(4800);
Serial1.begin(19200);
Serial2.begin(38400);
Serial3.begin(9600);
mag.initialize();
lcd.begin(16, 2);
if (!mag.isCalibrated()) {
if (!mag.isCalibrating()) { //And we're not currently calibrating
Serial.println("Entering calibration mode");
mag.enterCalMode(); //This sets the output data rate to the highest possible and puts the mag sensor in active mode
}
while (!mag.isCalibrated()) {
Serial2.write(CMD);
Serial2.write(SET_SPEED1);
Serial2.write(138);
Serial2.write(CMD);
Serial2.write(SET_SPEED2);
Serial2.write(118);
mag.calibrate();
}
}
}
void loop() {
while (mag.isCalibrated()) {
Serial.print("heading: "); Serial.println(mag.readHeading());
bool endTransmition = false;
while (Serial1.available()) {
c = (char)Serial1.read();
if (c != -1) {
input += c;
}
if (c == 10) {
int commaPosition = input.indexOf(',');
lat = input.substring(0, commaPosition);
lon = input.substring(commaPosition + 1, input.length());
latitude = lat.toFloat();
longitude = lon.toFloat();
endTransmition = true;
input = "";
}
}
while (Serial3.available()) {
if ( gps.encode(Serial3.read())) {
gps.f_get_position(&flat, &flon); // returns +/- latitude/longitude in degrees
}
}
if (flat != 0.0 && flon != 0.0 && endTransmition) {
/Waypoint distance calculation/
float R = 6371000.0;
flat = radians(flat);
latitude = radians(latitude);
float deltaLat = (latitude - flat);
float deltaLon = (longitude - flon);
float a = sin(deltaLat / 2) * sin(deltaLat / 2) + cos(flat) * cos(latitude) * sin(deltaLon / 2) * sin(deltaLon / 2);
float c = 2 * atan2(sqrt(a), sqrt(1 - a));
d = R * c;
//Serial.print("Distance to travel: "); Serial.println(d, 5);
/Waypoint bearing calculation/
float y = sin(deltaLon) * cos(latitude);
float x = cos(flat) * sin(latitude) - sin(flat) * cos(latitude) * cos(deltaLon);
float bearing = atan2(y, x);
bearing = degrees(bearing);
heading = -mag.readHeading();
int condition = (int)bearing - (int) heading;
int turnAngle = turn(condition);
if (turnAngle < 0) {
// Serial.println("--------------------");
// Serial.print("Bearing: "); Serial.println(bearing);
// Serial.print("Heading: "); Serial.println(heading);
// Serial.print("Turn Angle: "); Serial.println(turnAngle);
// Serial.println("--------------------");
while (true) {
// Serial.println("");
// Serial.print("Bearing: "); Serial.println(bearing);
// Serial.print("Heading: "); Serial.println(heading);
// Serial.print("Turn Angle: "); Serial.println(turnAngle);
// ("");
// Turn Left
if ((int) heading + 2 > (int) bearing && (int)bearing > (int) heading - 2) {
Serial2.write(CMD);
Serial2.write(SET_SPEED1);
Serial2.write(128);
Serial2.write(CMD);
Serial2.write(SET_SPEED2);
Serial2.write(128);
break;
}
Serial2.write(CMD);
Serial2.write(SET_SPEED1);
Serial2.write(118);
Serial2.write(CMD);
Serial2.write(SET_SPEED2);
Serial2.write(138);
}
}
if (turnAngle > 0) {
Serial.println("--------------------");
Serial.print("Bearing: "); Serial.println(bearing);
Serial.print("Heading: "); Serial.println(heading);
Serial.print("Turn Angle: "); Serial.println(turnAngle);
Serial.println("--------------------");
while (true) {
Serial.println("");
Serial.print("Bearing: "); Serial.println(bearing);
Serial.print("Heading: "); Serial.println(heading);
Serial.print("Turn Angle: "); Serial.println(turnAngle);
Serial.println("");
// Turn Right
if ((int) heading + 2 > bearing && (int) heading - 2 < bearing) {
Serial2.write(CMD);
Serial2.write(SET_SPEED1);
Serial2.write(128);
Serial2.write(CMD);
Serial2.write(SET_SPEED2);
Serial2.write(128);
break;
}
Serial2.write(CMD);
Serial2.write(SET_SPEED1);
Serial2.write(138);
Serial2.write(CMD);
Serial2.write(SET_SPEED2);
Serial2.write(118);
}
}
}
}
}
int turn (int provisional) {
if (provisional > 180) {
return provisional -= 360;
}
if (provisional <= - 180) {
return provisional += 360;
}
else {
return provisional;
}
}