here is the code that i am using for my car to easily navigate through waypoints but its not doing so its showing me right only why can any body help me here please
#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 = 25.37104;
float dest_longitude = 68.35567;
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) <65;
}
bool isLeftOfCenter() {
return (analogRead(potPin)/4) < 108;
}
bool isRightOfCenter() {
return (analogRead(potPin)/4) > 118;
}
bool isRight() {
return (analogRead(potPin)/4) > 128;
}
int beeperpin=44;
void setup() {
Serial1.begin(4800);
//Serial.begin(9600);
pinMode(leftMotor,OUTPUT);
pinMode(rightMotor,OUTPUT);
pinMode(motorA1,OUTPUT);
pinMode(motorA2,OUTPUT);
//Serial.print("GPS BASED`");
myLCD.begin(16,2);
myLCD.print("GPS BASED");
delay(3000);
myLCD.clear();
myLCD.home();
Wire.begin();
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1;
//pinMode(beeperpin,OUTPUT);
//pinMode(13,OUTPUT);
}
void loop(){
if (Serial1.available() > 0 ) {
// read incoming character from GPS
if (myGPS.decode(Serial1.read())) {
// check if the character completes a valid GPS sentence
// check if GPS positioning was active
if (myGPS.gprmc_status() == 'A') {
// float lat;
// float lon;
lat=myGPS.gprmc_latitude();
lon = myGPS.gprmc_longitude();
Serial.print("D:");
int d=(round(myGPS.gprmc_distance_to(dest_latitude, dest_longitude, MTR)));
// Serial.print(d);
// Serial.print("m");
myLCD.setCursor(0, 0);
myLCD.print(d);
myLCD.print('m');
if (d < 6){
myLCD.clear();
myLCD.home();
myLCD.setCursor(0,0);
myLCD.print("Destinat");
digitalWrite(13, HIGH);
digitalWrite(beeperpin, HIGH);
carbreak();
}
else if (d > 6){
myLCD.clear();
myLCD.home();
myLCD.setCursor(0, 0);
myLCD.print(d);
myLCD.print('m');// digitalWrite(beeperpin, LOW);
//digitalWrite(13, LOW);
float mydir= 0;
for (int x = 0; x<20; x=x+1){
mydir=mydir + calc_bearing (lat, lon, dest_latitude, dest_longitude);
}
mydir=mydir/20;
if (mydir < 0) {
mydir += 360;
}
if (mydir > 180) {
mydir -= 360;
}
float currentdir = compassvalue();
float finaldir =currentdir - mydir;
myLCD.setCursor(3,1);
if ( abs (finaldir) < 20)
{
// Serial.print ('=');
myLCD.print ("=");
steering('C');
delay(500);
motorForward();
}
else{
if (finaldir < 0)
{
// Serial.print ('<');
myLCD.print ("L");
steering('L');
delay(300);
motorForward();
delay(300);
carbreak();
delay(500);
}
if (finaldir > 0)
{
// Serial.print ('>');
myLCD.print ("R");
steering('R');
delay(300);
motorForward();
delay(300);
carbreak();
delay(500);
}
// Serial.print(abs(finaldir), DEC);
myLCD.print(abs(finaldir), DEC);
// myLCD.print(223, BYTE); // print °-character
}
}
}
}
}
}
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
}
