OdysseusRover Project With Arduino Uno.

ptuxiaki_v1_4.rar ptuxiaki_v1_4.rar
Size : 10.586 Kb
Type : rar


 

/*===================================================================================================================================================================================================
 ---------------------------------THEMA PTUXIAKHS ERGASIAS---------------------------
 AYTONOMO SYSTHMA POLHGHSHS KAI APOFUGEIS EMPODION ME XRISH  HLEKTRIKOU AMAKSIDIOU
 
 EPIBLEPON KATHIGITHS: ASTARAS ALEKSADROS 
 EKPONITHIKE APO: LLENGA ARBER, PATSONAS KONSTANTINOS
 COPYRIGHT 2011  BY DbagerS_Team
 ===================================================================================================================================================================================================
 
 ------------------------------------------description------------------------------------------
 motorPwm -->EINAI TO PWM TO DC KINHTHRA KAI EINAI SYNDEDEMENO STO DIGITAL PIN 5 
 motorA1 KAI motorA2 -->BITs ELEGXOU FORAS PERISTROFIS DC KINHTHRA  EINAI SUNDEDEMENA STA ANALOG PIN A1 & A2 ANTISTOIXA
trigerLeftSensor=8,trigerRightSensor=4,trigerFrontSensor=11 --->SKADALISMOS AISTHITIRION
 echoLeftSensor=6,echoRightSensor=12,echoFrontSensor=7 -->H  HXO APO TA AISTHITIRIA,TO PROTO STO AISTHITIRIO MPROSTA, TO DEUTERO GIA LAKOUBA ARISTERA KAI TO TRITO GIA LAKOUBA DEKSIA
 buttonPin--> PATONTAS TO BUTTON PERNOUME ENA STIGMA, SUNDEDEMENO STO ANALOG PIN A3
 servoAngle--> METABLITH H OPOIA PERIEXEI THN GONIA ME THN OPOIA THA PERISTAFEI O SERVO TOU AISTHITIRIOU  [H GONIA AUTH AKOLOUTHEI THN GONIA STHN OPOIA STRIBEI TO TIMONI]
 servoDir--> KATEUTHINSH POU PREPEI NA STRIPSEI TO TIMONI [0=ARISTERA, 1=EUTHEIA, 2=DEKSIA]
 matrixCounter--> METRITIS POU DEIXNEI THN DIEUTHINSH TOU EPOMENOU STIGMATOS POU EINAI APOTHIKEUMENO STHN EEPROM
 reading--> METAVLTITH POU KRATAME TIS MOIRES APO THN PIKSIDA
 ee--> METRITIS POU DEIXNEI THN DIEUTHINSH POY THA PREPEI NA GRAFTEI TO EPOMENO STIGMA STHN EEPROM
 counter=0 -->METRITIS POU METRAEI POSA STIGMATA EXOUN BRETHEI
 ifCounter=0--> SHMAIA POU DEIXNEI AN EINAI PATHMENO TO BUTTON H OXI
 directionInDeg--> GONIA POU PREPEI NA PERISTRAFEI GIA NA EUTHIGRAMISTEI ME TO ANTISTOIXO WAY-POINT
 noumWaypoints=EEPROM.read(500); -->METAVLITH H OPOIA KRATAEI TON ARITHMO TON WAY-POINT EITE TON APOTHIKEUOUME STHN ROM EITE TON DIAVAZOUME 
 latitude--> TREXON GEOGRAFIKPO PLATOS
 longitude-->  TREXON GEOGRFIKO MHKOS
 value--> METAVLITH H OPOIA BOHTHAEI STO GRAPSIMO STHN EEPROM
 nextLatitude--> TO GEOGRAFIKO PLATOS GIA TO WAY-POINT POU DIAVAZOUME APO THN EEPROM
 nextLongitude--> TO GEOGRAFIKO MHKOS GIA TO WAY-POINT POU DIAVAZOUME APO THN EEPROM
 ====================================================================================================================================================================================================
 */

#include "Ultrasonic.h"
#include <Servo.h>
#include <EEPROM.h>
#include <MsTimer2.h>
#include <LiquidCrystal.h>
#include <TinyGPS.h>
#include <Wire.h> 

LiquidCrystal lcd(4, 11, 3, 2, 12, 13); //LCD PIN SETUP


//=======================================
//SETTINGS
//=======================================
#define distanceError 0.00002        //EINAI H EMBELIA GURO APO THN OPOIA MPOREI NA BREI TO WAY-POINT
#define objectDistance1 200        //EINAI H MEGISTH APOSTASH STHN OPOIA 8EOROUME OTI BRE8IKE EMPODIO GIA ARISTERA KAI DEKSIA
#define objectDistance2 350        //EINAI H MEGISTH APOSTASH STHN OPOIA 8EOROUME OTI BRE8IKE EMPODIO GIA MPROSTA
#define xantakiDistance 30
#define pezouliDistance 20
#define highSpeed 140              //MEGISTH TAXUTHTA 
#define lowSpeed 135              //ELAXISTH TAXUTHTA [OTAN H APOSTASH APO TO WAY-POINT EINAI MIKROTERH TON 0.00015]
#define backSpeed 160              //TAXYTHTA OPIS8OPORIAS
#define objectSpeed 130          //TAXYTHTA APOFUGEIS EMPODIOU
#define obstacleWidth 100       // 1/2 PLATOS EMPODIOU SE CM
#define angleError 5            //MEIOSH GVNIA PROSPERASHS EMPODIOU      
int myservoCalib=-5;            //KALIMPRARISMA TIMONIOU
int servoSensorCalib=-5;
//========================================

#define M_PI 3.14159265358979323846                 //TO GNOSTO MAS π
#define maxDistance (objectDistance2+10)*29.1545*2  //MEGISTH APOSTASH POU METRAEI TO AISTHITIRO MPROSTA
#define left 140
#define right 40
#define str8 90

//=================================================================================================================
//GLOBAL VARIABLES
//=================================================================================================================
int motorPwm=5,motorA1=A1,motorA2=A2,trigerLeftSensor=8,trigerRightSensor=4,trigerFrontSensor=11,echoLeftSensor=6,echoRightSensor=12,echoFrontSensor=7,servoAngle,servoDir='S';
int frontSensorVal=500,frontSensorFi=500,leftSensorVal=30,rightSensorVal=30,matrixCounter=0,reading,ee,counter=0,ifCounter=0,directionInDeg,buttonPin=A3,noumWaypoints=EEPROM.read(500);
float latitude, longitude,value,nextLatitude,nextLongitude;
float* eepromMatrix = (float*)malloc(sizeof (float) * (noumWaypoints*2));
Servo myservo,servoSensor;                         
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps(),flagObject=0;
//===================================================================================================================

void setup()
{
  Wire.begin();                     //NEEDED FOR COMPASS
  lcd.begin(16,2);                  //LCD SETUP
  myservo.attach(9,900,2100);       //ATTACH STEERING SERVO
  servoSensor.attach(10);           //ATTACH SERVO FOR THE SENSOR
  myservo.write(90+myservoCalib);   //GO TO CENTER
  servoSensor.write(90+myservoCalib);   //GO TO CENTER  
  pinMode(buttonPin,INPUT);         //BUTTON FOR GETTING WAYPOINTS
  pinMode(motorPwm,OUTPUT);         //SETUP DC MOTOR
  pinMode(motorA1,OUTPUT);          //PIN TO CONTROL THE DIRECTION OF DC MOTOR
  pinMode(motorA2,OUTPUT);          //PIN TO CONTROL THE DIRECTION OF DC MOTOR
  Serial.begin(4800);               //SETUP SERIAL MONITOR AND GPS
  actionMode();                     //LCD MENU
  lcd.noDisplay();                  //TURN OFF LCD TO USE PIN 12 FOR RIGHT GAP SENSOR
}

void loop()
{
  //servoDir='S';
  //directionInDeg=0;


  steering(0);   //ME PERASMA TIMHS 0 H PLOHGHSH GINETE APO TO GPS
  readAllSensors();
 // gapRoutine();    //ROUTINA EURESHS KAI APOFIGEIS XANTAKION
  newServoSensor();  //ROUTINA ELGXOU YPARKSEIS KAI APOFUGEIS EMPODION
  readGps();
  navigationRead();
  
  
}




void readGps()
{
  bool newdata = false;

  if(feedgps())
    newdata = true;

  if (newdata)
  {
    gpsdump(gps);
  }
  
}

void gpsdump(TinyGPS &gps)
{
  gps.f_get_position(&latitude, &longitude, NULL);
  Serial.print(latitude,5);
  Serial.print("   ");
  Serial.println(longitude,5);

}

bool feedgps()
{
  while (Serial.available())
  {
    if (gps.encode(Serial.read()))
      return true;
  }
  return false;
}
//=========================================================
// ROUTINA KA8ISTERISEIS H OPOIA EKMETALEYETAI 
// TON XRONO DIAVAZONTAS TO GPS
//=========================================================
void delayViaGps(int timeDelay)
{
  unsigned long start = millis();
  while((millis()-start<timeDelay))
  {
    readGps();
  }
}


//=======================
//VARIABLES FOR KEYBOARD
//=======================
int analogInputPin=0;
int totalOutputDigit=9;
int KeyForEnder='#';
int keyForClear='*';
int rowToPrint=0;
int columnToPrint=0;
int IncomingString;
//=================================================
// ROUTINA EMFANISHS TOU MENOU EPILOGON STHN LCD 
//===================================================
int  actionMode()
{

  lcd.clear();
  lcd.print("Navig. Or Truck.");
  lcd.setCursor(0,1);
  lcd.print("press 1 or 2: ");
  int rowToPrint=15;
  int columnToPrint=1;
  int ti_pati8ike=getPressString(IncomingString,analogInputPin,KeyForEnder,keyForClear,rowToPrint,columnToPrint);

  if(ti_pati8ike==1)
  {
    lcd.clear();
    lcd.print("No. Of Waypoints");
    int rowToPrint=7;
    int columnToPrint=1;
    noumWaypoints=getPressString(IncomingString,analogInputPin,KeyForEnder,keyForClear,rowToPrint,columnToPrint);
    lcd.clear();
    EEPROM.write(500,noumWaypoints);
    navigationWrite();
    eepromMatrixRead();
    readNextWaypoint4firstTime();
    return noumWaypoints;
  }
  else if(ti_pati8ike==2)
  {
    noumWaypoints=EEPROM.read(500);
    eepromMatrixRead();
    readNextWaypoint4firstTime();
    return noumWaypoints;
  }
  else
  {
    actionMode();
  }
}
void readNextWaypoint4firstTime()
{
//  Serial.print("matrixCounter");
//  Serial.println(matrixCounter);
  nextLatitude=eepromMatrix[matrixCounter];
//  Serial.print("Saved lat: ");
//  Serial.print("   ");
//  Serial.print(nextLatitude,5);
//  Serial.print("          ");
  matrixCounter++;
  nextLongitude=eepromMatrix[matrixCounter];
//  Serial.print("Saved Log: ");
//  Serial.print("   ");
//  Serial.print(nextLongitude,5);
//  Serial.println("        ");
  matrixCounter++;
}

float eepromMatrixRead()
{  
  for(int i=0;i<(noumWaypoints*2);i++)    //*2 dioti exoume long,lat 
  {
    eepromMatrix[i]=EEPROM_readfloat();
  }
}


void EEPROM_writefloat(float value)
{
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(value); i++)
    EEPROM.write(ee++, *p++);
}

float EEPROM_readfloat()
{
  float value = 0.0;
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(value); i++)
    *p++ = EEPROM.read(ee++);
  return value;
} 




//=============================================================================
//KATHOS STRIVEI ELEGXEI AN UPARXEI EMPODIO PROS TIN KATEUTHINSI AUTI
void avoidObstacleRout(int rotation)
{
  int valCompass=getAngleInDeg(),finalVal,beginVal;
  beginVal=valCompass;
  double fi;
      fi=atan2(obstacleWidth,frontSensorFi);    //YPOLOGIZOUME THN GONIA POU SXHMATIZETAI APO TO TREXON KAI APO TO APOTHIKEUMENO GEGRAFIKO MHKOS KAI PLATOS
      fi=((fi*180)/M_PI); 
      if(fi>65)
      {
        fi=65;
      }
      Serial.print("fi= ");Serial.println(fi);
      if(rotation=='R' && (valCompass+fi)>=360)
      {
        finalVal=(valCompass+fi)-360;
      }
      else if(rotation=='R' && (valCompass+fi)<360)
      {
        finalVal=valCompass+fi;
      }
      else if(rotation=='L' && valCompass<fi)
      {
        finalVal=360-(fi-valCompass);
      }
      else if (rotation=='L' && valCompass>fi)
      {
        finalVal=valCompass-fi;
      }
      Serial.print("finalVal= ");Serial.println(finalVal);
      if(rotation=='R')
      {
        int remainder=angleError+1;
        while(remainder>0)
        {
          if ((valCompass+fi)>=360 &&(beginVal+fi)>360)
          {
            remainder=finalVal-(valCompass-360); //ARNITIKO PROSIMO
          }
          else
          {
            remainder=finalVal-valCompass;
          }
          Serial.print("remainder= ");Serial.println(remainder);
          rightSensor();
          leftSensor();
          frontSensor();  
          if(frontSensorVal<objectDistance1 || (leftSensorVal>xantakiDistance || rightSensorVal>xantakiDistance || leftSensorVal<pezouliDistance || rightSensorVal<pezouliDistance)) //BRI8IKE XANTAKI
          {
            motorStop();
            delay(1000);
            break;
          }
          valCompass=getAngleInDeg();
        }
      }
      else if(rotation=='L')
      {
        int remainder=360;
        while(remainder>0)
        {
          if(valCompass<fi && (beginVal-fi)<0)
          {
            remainder=(360-finalVal)+valCompass;
          }
          else
          {
            remainder=valCompass-finalVal;
          }
          Serial.print("remainder= ");Serial.println(remainder);
          rightSensor();
          leftSensor();
          frontSensor();  
          if(frontSensorVal<objectDistance1 || (leftSensorVal>xantakiDistance || rightSensorVal>xantakiDistance || leftSensorVal<pezouliDistance || rightSensorVal<pezouliDistance)) //BRI8IKE XANTAKI
          {
            motorStop();
            delay(1000);
            break;
          }
          valCompass=getAngleInDeg();
        }
      }
}


//=======================================================================================================================================
//ROUTINA H OPOIA BRISKEI PROS POIO TETARTIMORIO KINHTE TO AMAKSAKI THN KATEUTHINSH POU PREPEI NA STRIPSEI KAI TIS MOIRES OSTE NA VREI TO
//WAY-POINT
//=======================================================================================================================================
float angleNextPoint;
int diaforaGonion;
int directionOfAngle()
{
  getAngleInDeg();
  // PROTO TETARTIMORIO
  if((nextLatitude-latitude)>=0 && (nextLongitude-longitude)>0)
  {
    float sintAtan1=fabs(nextLongitude-longitude);   //DIAFORA METAKSI TREXON KAI APOTHIKEUMENOU GEOGRAFIKOU MHKOUS
    float sintAtan2=fabs(nextLatitude-latitude);    //DIAFORA METAKSI TREXON KAI APOTHIKEUMENOU GEOGRAFIKOU PLATOUS
    angleNextPoint=atan2(sintAtan1,sintAtan2);    //YPOLOGIZOUME THN GONIA POU SXHMATIZETAI APO TO TREXON KAI APO TO APOTHIKEUMENO GEGRAFIKO MHKOS KAI PLATOS

    angleNextPoint=((angleNextPoint*180)/M_PI);   //METATROPH THS GONIAS APO AKTINIA SE MHRES
    diaforaGonion=fabs(angleNextPoint-reading);  //DEAFORA GONION SE SXESH ME AUTH POU YPOLOGISAME AP AUTH POU MAS DINEI H PIKSIDA

//ELEGXOS SE POIO TETARTIMORIO PAME
    if(reading>angleNextPoint && diaforaGonion<180)
    {
      servoDir='L';
      return(reading-angleNextPoint);
    }
    else if(reading>angleNextPoint && diaforaGonion>=180)
    {
      servoDir='R';
      return(360-reading+angleNextPoint);
    }
    else if(reading<angleNextPoint)
    {
      servoDir='R';
      return(angleNextPoint-reading);
    }
    else
    {
      servoDir='S';
      return(0);
    }
  }
  //DEUTERO TETARTIMORIO
  else if((nextLatitude-latitude)<0 && (nextLongitude-longitude)>=0)
  {
    float sintAtan1=fabs(nextLongitude-longitude);
    float sintAtan2=fabs(nextLatitude-latitude);
    angleNextPoint=atan2(sintAtan2,sintAtan1);

    angleNextPoint=((angleNextPoint*180)/M_PI)+90;
    diaforaGonion=fabs(angleNextPoint-reading);

    if(reading>angleNextPoint && diaforaGonion<180)
    {
      servoDir='L';
      return(reading-angleNextPoint);
    }
    else if(reading>angleNextPoint && diaforaGonion>=180)
    {
      servoDir='R';
      return(360-reading+angleNextPoint);
    }
    else if(reading<angleNextPoint)
    {
      servoDir='R';
      return(angleNextPoint-reading);
    }
    else
    {
      servoDir='S';
      return(0);
    }
  }
  //TRITO TETARTIMORIO
  else if((nextLatitude-latitude)<=0 && (nextLongitude-longitude)<0)
  {
    float sintAtan1=fabs(nextLongitude-longitude);
    float sintAtan2=fabs(nextLatitude-latitude);
    angleNextPoint=atan2(sintAtan1,sintAtan2);

    angleNextPoint=((angleNextPoint*180)/M_PI)+180;
    diaforaGonion=fabs(angleNextPoint-reading);

    if(reading>angleNextPoint)
    {
      servoDir='L';
      return(reading-angleNextPoint);
    }
    else if(reading<angleNextPoint && diaforaGonion>=180)
    {
      servoDir='L';
      return(360-angleNextPoint+reading);
    }
    else if(reading<angleNextPoint && diaforaGonion<180)
    {
      servoDir='R';
      return(angleNextPoint-reading);
    }
    else
    {
      servoDir='S';
      return(0);
    }
  }
  //TETARTO TETARTIMORIO
  else if((nextLatitude-latitude)>0 && (nextLongitude-longitude)<=0)
  {
    float sintAtan1=fabs(nextLongitude-longitude);
    float sintAtan2=fabs(nextLatitude-latitude);
    angleNextPoint=atan2(sintAtan2,sintAtan1);

    angleNextPoint=((angleNextPoint*180)/M_PI)+270;
    diaforaGonion=fabs(angleNextPoint-reading);

    if(reading>angleNextPoint)
    {
      servoDir='L';
      return(reading-angleNextPoint);
    }
    else if(reading<angleNextPoint && diaforaGonion>=180)
    {
      servoDir='L';
      return(360-angleNextPoint+reading);
    }
    else if(reading<angleNextPoint && diaforaGonion<180)
    {
      servoDir='R';
      return(angleNextPoint-reading);
    }
    else
    {
      servoDir='S';
      return(0);
    }
  }

}
//====================================
//ROUTINA POU DIAVAZEI THN PIKSIDA
//===================================
int getAngleInDeg() 
{ 
  Wire.beginTransmission(63);
  Wire.send(0x41);             
  Wire.endTransmission();
  delayViaGps(10);
  Wire.requestFrom(63, 2); 
  byte MSB = Wire.receive();
  byte LSB = Wire.receive();
  reading = ((MSB << 8) + LSB)/10; 
  Serial.print("       reading        ");Serial.println(reading);
  return(reading);
}




void gapRoutine()
{
  if (leftSensorVal>xantakiDistance || rightSensorVal>xantakiDistance || leftSensorVal<pezouliDistance || rightSensorVal<pezouliDistance) //BRI8IKE XANTAKI
  {
    Serial.print("BRETHIKE XANTAKI ");Serial.println(leftSensorVal);Serial.print("   ");Serial.println(rightSensorVal);
    motorStop();    // KANO STOP
    motorBackward(100);
    delay(600);
    motorStop();
    delay(1000);
    leftSensor();
    delayViaGps(100);
    rightSensor();
    if ((leftSensorVal>xantakiDistance && rightSensorVal>xantakiDistance) || ((leftSensorVal<pezouliDistance && rightSensorVal<pezouliDistance))) //TO XANTAKI EINAI KA8ETA MPROSTA MOY GIATI BRISKOUN KAI TA DUO 
    {                                                                   //AIS8ITIRIA
      if(servoDir=='L')
      {
        myservo.write(right+myservoCalib);
      }
      else
      {
        myservo.write(left+myservoCalib);
      }
      motorBackward(backSpeed); //OPISTHOPORIA ME TIS RODES ESTI OPOS HTAN OTAN EFTASE
      delay(2000);        
      motorStop();
      delay(200);

    }
    else if(leftSensorVal>xantakiDistance || leftSensorVal<pezouliDistance) // SE PERIPTOSH POU VREI MONO TO ARISTRERO
    {
      motorBackward(backSpeed);
      delay(3000);
      motorStop();
      myservo.write(right+myservoCalib);
      motorForward(highSpeed);
      //avoidObjectDelay();            //PAME MPROSTA KAI ANTI GIA APLH DELAY ELEGXOUME KAI TI SERVOSENSOR
      delay(2000);
    }
    else if(rightSensorVal>xantakiDistance || rightSensorVal<pezouliDistance) //SE PERIPTOSH POU VREI MONO DEKSIA
    {
      motorBackward(backSpeed);
      delay(3000);
      motorStop();
      myservo.write(left+myservoCalib);
      motorForward(highSpeed);
      //avoidObjectDelay();  ////PAME MPROSTA KAI ANTI GIA APLH DELAY ELEGXOUME KAI TI SERVOSENSOR
      delay(2000);
    }
  }
}


unsigned long getPressString(int pressKey,int analogInputPin,int KeyForEnder, int keyForClear,int rowToPrint,int columnToPrint)
{
  //DILOSI METAVLITON
  int keyPressCounter=0;
  int digitalValue=KeyForEnder,analogValue; //vazo digitalValue=KeyForEnder etsi oste an kratao patimeno to ENTER na min ektelei sunexos tin main
  int preDigitalValue='nothing';
  long keyBoardString=0;

readAnalogInput: 
  //lcd.blink();
  for(int i=0;i<10;i++)
  { //DIAVAZEI TIN ANALOGIKI EISODO //I DELAY EINAI GIA NA PROLAVEI NA KANEI TIN METATROPI
    analogValue=analogRead(analogInputPin);
    delay(10);
  }
  
  preDigitalValue=digitalValue;
    if(50<analogValue && analogValue<60)
    {
      digitalValue=1;
    }
    else if(97<analogValue && analogValue<107)
    {
      digitalValue=2;
    }
    else if(142<analogValue && analogValue<152)
    {
      digitalValue=3;
    }
    else if(190<analogValue && analogValue<205)
    {
      digitalValue=4;
    }
    else if(319<analogValue && analogValue<329)
    {
      digitalValue=5;
    }
    else if(418<analogValue && analogValue<428)
    {
      digitalValue=6;
    }
    else if(543<analogValue && analogValue<553)
    {
      digitalValue=7;
    }
    else if(700<analogValue && analogValue<717)
    {
      digitalValue=8;
    }
    else if(784<analogValue && analogValue<796)
    {
      digitalValue=9;
    }
    else if(925<analogValue && analogValue<935)
    {
      digitalValue=0;
    }
    else if(860<analogValue && analogValue<870)
    {
      digitalValue='*';
    }
    else if(955<analogValue && analogValue<965)
    {
      digitalValue='#';
    }
    else
    {
      digitalValue='nothing';
    }
  if(digitalValue==preDigitalValue || digitalValue=='nothing') 
  {
    goto readAnalogInput;
  }     
  else if(digitalValue==KeyForEnder)//AN PATITHIKE TO ENDER (SUNITHOS EINAI TO D)
  {
    keyPressCounter=0;
    lcd.noBlink();
    goto endGetPressKeyFunction;
  }
 else if(digitalValue==keyForClear)//AN PATITHIKE TO CLEAR (SUNITHOS EINAI TO C)
  {
    keyBoardString=keyBoardString/10;
    lcd.clear();
    lcd.print(keyBoardString);
    if(keyBoardString==0)
    {
      keyPressCounter=0;
    }
    else
    {
      keyPressCounter--;
    }
    goto readAnalogInput;
  }
  if(keyPressCounter>8)
  {
    goto readAnalogInput;
  }
  keyBoardString=(keyBoardString*10)+digitalValue;
  Serial.println(keyBoardString);
  //lcd.clear();
  lcd.setCursor(rowToPrint,columnToPrint);
  lcd.print(keyBoardString);
  keyPressCounter++;
  goto readAnalogInput;
endGetPressKeyFunction:
  return(keyBoardString);
}








//======================================================
// ROUTINA H OPOIA KINEI MPROSTA TO AMAKSSAKI ME TAXYTHTA
// POU DINOUME EMEIS OTAN THN KALOUME
//========================================================
void motorForward(int timi)
{
  analogWrite(motorPwm,0);
  digitalWrite(motorA1,HIGH);
  digitalWrite(motorA2,LOW);
  delayViaGps(10);
  analogWrite(motorPwm,timi);
}
//==================================================================
// ROUTINA H OPOIA KINEI ME TO AMAKSAKI PROS TA PISO ME TAXYTHTA POU
// TOU PRNAME ME TO KALESMA THS ROUTINAS
//==================================================================
void motorBackward(int timi)
{
  analogWrite(motorPwm,0);
  digitalWrite(motorA1,LOW);
  digitalWrite(motorA2,HIGH);
  delayViaGps(10);
  analogWrite(motorPwm,timi);
  
}
//===========================================
// ROUTINA H OPOIA KANEI FRENARISMA
//===========================================
void motorStop()
{
 
  analogWrite(motorPwm,0);
  digitalWrite(motorA1,HIGH);
  digitalWrite(motorA2,HIGH);
   delayViaGps(60);
}


int navigationWrite()
{
looop:
  readGps();
  lcd.home();            //EMFANISH TOU TREXON GEOGRAFIKOU PLATOUS KAI MHKOUS STHN
  lcd.setCursor(0,0);    // LCD KATA THN DIARKEIA POU PERNOUME TA STEIGMA
  lcd.print(latitude,5);
  lcd.setCursor(0,1);
  lcd.print(longitude,5);
  if((counter+1)>noumWaypoints)      //ELEGXON AN O ARITHMOS TON STIGMATON POU PEIRAME EINAI IDIOS 
  {                             // ME AUTON POU EPILEKSAME
    ee=0;
    counter=0;
    return(0);
  }
  while(digitalRead(buttonPin)==LOW)
  {
    if(ifCounter==0)
    {
      lcd.clear();
      lcd.print(" ok  ");lcd.print((counter+1));
      //geografiko platos
      value=latitude;
      EEPROM_writefloat(value);
      //geografiko mikos
      value=longitude;
      EEPROM_writefloat(value);

      counter++;
      ifCounter=1;
    }
  }
  ifCounter=0;
 goto looop;

}


//==============================================================================
//ROUTINA POU DIAVAZEI TO GPS OTAN TO AMAKSAKI PROSPATHEI NA PTASEI STO WAY-POINT
//===============================================================================
void navigationRead()
{
  directionInDeg=directionOfAngle();  //KALOUME THN ROUTINA POU MAS DINEI MAS EPISTREFEI THN KATEYTHINSH STHN OPOIA PREPEI NA STRIPSOUME ALLA KAI POSES MOIRES
  if(directionInDeg<6)
  {
    servoDir='S';       //AN I GONIA EINAI MIKROTERI TON 6 MOIRON TOTE TO STELNOUME STR8
  }
  identifyWaypoint();
}
//================================================================
//ROUTINA ELEGXOU GIA TO AN FTASAME STO EPITHIMITO SHMEIO [WAY-POINT]
//====================================================================
void identifyWaypoint()
{    //ELEGXOS AN FTASAME STO EPITHIMITO GEOGRAFIKO MHKOS KAI PLATOS
  if(((nextLongitude-longitude)<distanceError && (nextLongitude-longitude)>-distanceError) && ((nextLatitude-latitude)<distanceError && (nextLatitude-latitude)>-distanceError))
  {
    nextLatitude=eepromMatrix[matrixCounter];
    Serial.print("Saved lat: ");
    Serial.print("   ");
    Serial.print(eepromMatrix[matrixCounter],5);
    Serial.print("          ");
    matrixCounter++;
    nextLongitude=eepromMatrix[matrixCounter];
    Serial.print("Saved Log: ");
    Serial.print("   ");
    Serial.print(eepromMatrix[matrixCounter],5);
    Serial.println("        ");
    matrixCounter++;
    Serial.print("good, go to next!!!!!!");
    Serial.println(counter);
    counter++;
    if(counter==EEPROM.read(500))  //AN TA SHMEIA POY BRETHIKA EINAI ISA ME TA SHMEIA POU APOTHIKEUSAME TOTE OLOKLHROTHIKE H DIADROMH
    {
      Serial.println("message.navigation.complete");
halt:

      motorStop();
      goto halt;
    }
  } 
}


//=============================================================
//ROUTINA EURESHS KAI APOFIGHS EMPODION
//=============================================================
void newServoSensor()
{

  //=================================================== 
  //EXEI EMPODIO MPRASTA  
  //=================================================== 
  if(frontSensorVal<=objectDistance2)   //ELEGXOS GIA THN YPARKSEI EMPODIOU
  {
    motorForward(0);
    if(servoAngle<70)            //AN  TO AMAKSAKI PAEI ARISTERA TOTE
    {
      delayViaGps(300);
      servoSensor.write(str8+servoSensorCalib);    //ELEGXOUME KAI EYTHEIA
      delayViaGps(300);
      frontSensor();
      if(frontSensorVal<=objectDistance2)  //AN EXEI KAI EUTHEIA EMPODIO TOTE 
      { 
        motorStop();
        delayViaGps(100);
        servoSensor.write(right+servoSensorCalib);     // KOITAME KAI DEKSIA
        delayViaGps(300);
        frontSensor();
        if(frontSensorVal<=objectDistance1)   //AN EXEI KAI DEKSIA TOTE KANOUME STOP
        {        
          Serial.print("BRETHIKE DEKSIA STA ");
          Serial.println(frontSensorVal);
          motorStop();
          delayViaGps(100);
          myservo.write(left+servoSensorCalib);          //PRIN KANEI OPISTHEN GIRIZEI TO TIMONI APO THN ANTITHETI KATEYTHINSH SE SXESH ME TO WAY-POINT
          motorBackward(backSpeed);  //STO OPOIO PREPEI NA PAEI GIA BA KATAFEREI NA APEGLOVISTEI. 
          delayViaGps(2800);
          motorStop();
          delayViaGps(100);
          flagObject=1;
        }
        else
        {
          steering(right);
        }
      }
      else
      {
        steering(str8);
      }
    }
    else if(servoAngle>=70 && servoAngle<=110)  //AN TO AMAKSAKI PAEI SXEDON EYTHIA APO 70 OS 110 MOIRES
    {
      if(servoDir=='L')         //AN TO DIR EINAI =0 TOTE PROTEREOTITA EXEI NA KOITAKSOUME ME TO AISTHITIRIO PROTA APO 
      {                       // APO ARISTERA GIA YPARKSEI EMPODIOU, OSTE NA KINHTHOUME PROS TO WAY-POINT
        delayViaGps(300);
        servoSensor.write(left+servoSensorCalib);
        delayViaGps(300);
        frontSensor();
        if(frontSensorVal<=objectDistance1)   
        {      
          motorStop();
          delayViaGps(300);
          servoSensor.write(right+servoSensorCalib);
          delayViaGps(300);
          frontSensor();
          if(frontSensorVal<=objectDistance1)
          {
            motorStop();
            delayViaGps(300);
            myservo.write(left+servoSensorCalib);            //PRIN KANEI OPISTHEN GIRIZEI TO TIMONI APO THN ANTITHETI KATEYTHINSH SE SXESH ME TO WAY-POINT
            motorBackward(backSpeed);    //STO OPOIO PREPEI NA PAEI GIA BA KATAFEREI NA APEGLOVISTEI. 
            delayViaGps(2800);
            motorStop();
            delayViaGps(100);
            flagObject=1;
          }
          else
          {
            steering(right);
          }
        }
        else
        {
          steering(left);
        }
      }
      else if(servoDir=='S' || servoDir=='R')  //AN H KATEUTHINSH EINAI EYTHEIA H DEKSIA TOTE PROTEREOTHTA EXEI TO NA EREUNISOUME GIA EMPODIO
      {                                    // PROTA APO DEKSIA OSTE NA KINHTHOUME PROS TO WAY-POINT
        servoSensor.write(right+servoSensorCalib);
        delayViaGps(600);
        //delay(1000);
        frontSensor();
        if(frontSensorVal<=objectDistance1)
        {
          motorStop();
          servoSensor.write(left+servoSensorCalib);
          delayViaGps(600);
          //delay(1000);
          frontSensor();
          if(frontSensorVal<=objectDistance1)
          {
            motorStop();
            delayViaGps(300);
            myservo.write(right+servoSensorCalib);        //PRIN KANEI OPISTHEN GIRIZEI TO TIMONI APO THN ANTITHETI KATEYTHINSH SE SXESH ME TO WAY-POINT
            motorBackward(backSpeed);  //STO OPOIO PREPEI NA PAEI GIA BA KATAFEREI NA APEGLOVISTEI. 
            delayViaGps(2800);
            motorStop();
            delayViaGps(100);
            flagObject=1;
          }
          else
          {
            steering(left);
          }
        }
        else
        {
          steering(right);
        }
      }
    }
    else //gonia > 110  AN TO AMAKSAKI STRIBEI PROS TA DEKSIA TOTE
    {
      delayViaGps(300);
      servoSensor.write(str8+servoSensorCalib);  //ELEGXOUME PROTA EUTHEIA
      delayViaGps(300);
      frontSensor();
      if(frontSensorVal<=objectDistance2)  //AN YPARXEI EMPODIO  MPROSTA TOTE
      {
        motorStop();        
        servoSensor.write(left+servoSensorCalib);  // ELEGXOUME ARISTERA 
        delayViaGps(300);
        frontSensor();
        if(frontSensorVal<=objectDistance1)   //KAI AN YPARXEI KAI ARISTERA TOTE KANOUME STOP
        {
          motorStop();
          delayViaGps(300);
          myservo.write(right+servoSensorCalib);        //PRIN KANEI OPISTHEN GIRIZEI TO TIMONI APO THN ANTITHETI KATEYTHINSH SE SXESH ME TO WAY-POINT
          motorBackward(backSpeed);  //STO OPOIO PREPEI NA PAEI GIA BA KATAFEREI NA APEGLOVISTEI. 
          delayViaGps(2800);
          motorStop();
          delayViaGps(100);
          flagObject=1;
        }
        else
        {
          steering(left);
        }
      }
      else
      {
        steering(str8);
      }
    }
  }
}

void readAllSensors() //diakopi kathe 50msec
{
  servoAngle=servoAngleRout(servoDir,directionInDeg);
  servoSensor.write(servoAngle);
  rightSensor();
  leftSensor();
  frontSensor();  
  frontSensorFi=frontSensorVal;    //METABLITH GIA NA YPOLOGISIYME TO FI
  //delayViaGps(10);
}

 void leftSensor()
{
  Ultrasonic leftSensor(trigerLeftSensor,echoLeftSensor);
  leftSensorVal=leftSensor.Ranging(CM);
  //Serial.print("                                         left: "); Serial.println(leftSensorVal);
  if(leftSensorVal>500)
  {
    leftSensorVal=xantakiDistance;
  }
}
void rightSensor()
{
  Ultrasonic rightSensor(trigerRightSensor,echoRightSensor);
  rightSensorVal=rightSensor.Ranging(CM);
  //Serial.print("                                                            right: "); Serial.println(rightSensorVal);
  if(rightSensorVal>500)
  {
    rightSensorVal=xantakiDistance;
  }
}
void frontSensor()
{
  Ultrasonic frontSensor(trigerFrontSensor,echoFrontSensor);
  frontSensorVal=frontSensor.Ranging(CM);
  //Serial.print("                                                front: "); Serial.println(frontSensorVal);
}


//============================================================
//ROUTINA H OPOIA ELEGXEI TO TIMONI 
//===========================================================
void steering(int stat)
{ 
  if(stat==0 && flagObject==0)   // TO AMAKSSAKI PLOHGITE ME BASI TO GPS        
  {                             //AN H APOSTASH APO TO WAY-POINT EINAI MIKROTERH AUTOU TOU ORIOU TOTE ELATONOUME TAXYTHTA
    if(((nextLongitude-longitude)<0.00015 && (nextLongitude-longitude)>-0.00015 ) && ((nextLatitude-latitude)<0.00015  && (nextLatitude-latitude)>-0.00015 ))
    {
      motorForward(lowSpeed);
    }
    else
    {
      motorForward(highSpeed);
    }
    myservo.write(servoAngleRout(servoDir,directionInDeg)+myservoCalib);
  }
  else if(stat==left)  //EXEI VREI EMPODIO MPRASTA. STRIVEI 40 MOIRES TO TIMONI GIA NA APOFUGEI TO EMPODIO
  {
    motorForward(objectSpeed);
    myservo.write(left+myservoCalib);
    servoSensor.write(left+servoSensorCalib);
    avoidObstacleRout('L');
  }
  else if(stat==str8)  //EXEI VREI EMPODIO MPRASTA. STRIVEI 90 MOIRES TO TIMONI GIA NA APOFUGEI TO EMPODIO
  {
    myservo.write(str8+myservoCalib);
    motorForward(objectSpeed);
    delay(2000);
  }
  else if(stat==right)  //EXEI VREI EMPODIO MPRASTA. STRIVEI 140 MOIRES TO TIMONI GIA NA APOFUGEI TO EMPODIO
  {
    motorForward(objectSpeed);
    
    myservo.write(right+myservoCalib);
    servoSensor.write(right+servoSensorCalib);
    avoidObstacleRout('R');
  }
  flagObject=0;
}

int servoAngleRout(int Dir,int Angle)
{
  int servoAngle;
  if(Dir=='L')
  {
    if(Angle<50)
    {
      servoAngle=str8+Angle;
    }
    else
    {
      servoAngle=left;
    }
  }
  else if(Dir=='S')
  {
    servoAngle=str8;
  }
  else if(Dir=='R')
  {
    if(Angle<50)
    {
      servoAngle=str8-Angle;
    }
    else
    {
      servoAngle=right;
    }
  }
  return servoAngle;
}

This free website was made using Yola.

No HTML skills required. Build your website in minutes.

Go to www.yola.com and sign up today!

Make a free website with Yola