RC Android Rover with GPS Tracker Using DaguRobot and ARTe

#include <Ultrasonic.h>
#include <Motor.h>
#include <WiFiESP8266.h>
#include <Adafruit_GPS.h>
// Attach GPS module to Arduino Due Serial2
#define GPSSerial Serial2
#define PMTK_SET_NMEA_UPDATE_1HZ  "$PMTK220,1000*1F"
#define PMTK_SET_NMEA_UPDATE_5HZ  "$PMTK220,200*2C"
#define PMTK_SET_NMEA_UPDATE_10HZ "$PMTK220,100*2F"

// turn on only the second sentence (GPRMC)
#define PMTK_SET_NMEA_OUTPUT_RMCONLY "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"
// turn on GPRMC and GGA
#define PMTK_SET_NMEA_OUTPUT_RMCGGA "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"
// turn on ALL THE DATA
#define PMTK_SET_NMEA_OUTPUT_ALLDATA "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28"
// turn off output
#define PMTK_SET_NMEA_OUTPUT_OFF "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"
#define PMTK_Q_RELEASE "$PMTK605*31"
//------------------------------------------------
// Pins connected to Ultrasonic
const int ul_trig = 43;
const int ul_eco = 41;
// Pins connected to H-bridge to drive motors
const int mR_en = 3;
const int mL_en = 2;
const int mR_A = 31;
const int mR_B = 30;
const int mL_A = 33;
const int mL_B = 32;
// Pins connected to Encoder
const int enR_sen = 24;
const int enL_sen = 26;
// Wi-Fi module attached to Arduino Due Serial1
// Netwoek parameters
String Network = "INO_ESP8266";
String Password = "12345678";
String KeyType = "2";
String Channel = "1";
//------------------------------------------------
// Duty Cycle Max and Min used to PWM driving of motors :
const int dc_min = 3000;
const int dc_max = 4096;
// Encoder variables
const int CFL = 20;         // Rising for a lap
static unsigned long cL;    // Left rising counter
static unsigned long cR;    // Right rising counter
static unsigned long turnTimeL; //Time used to control right turn of vehicle
static unsigned long turnTimeR; //Time used to control left turn of vehicle
static unsigned long t;     // Arduino time
static float rpsL;          // Estimated Left wheel velocity
static float rpsR;          // Estimated Right wheel velocity
static int dcL;             // Duty cycle used for left wheel when control is active
static int dcR;             // Duty cycle used for right wheel when control is active


// Buffer used to store the data recived from Wi-Fi
String Buffer;
// Buffer used to store the data will send by Wi-Fi
String Data;
// Buffer used to store the command recived from Wi-Fi or from PC
String Command;
// Buffer used to store the data recived from GPS
String BufferGPS;

static bool start;      // set to true after Wi-Fi module initialization
static bool wifiCon;    // set to true after a client is connected to network
static bool okfound;    // set to true during  Wi-Fi module initialization
static bool readyfound; // set to true during  Wi-Fi module initialization
static bool motFon;     // set to true when vheicle go forward
static bool motBon;     // set to true when vheicle go back
static bool motRotR;    // set to true when vheicle turn Left
static bool motRotL;    // set to true when vheicle turn Right
static bool controlStartL;  // set to true when control of left  wheel is enable
static bool controlStartR;  // set to true when control of right  wheel is enable
static bool GPSstart;       // set to true after GPS module initialization
static bool GPSfix;         // set to true after reciving a fix from GPS
//------------------------------------------------
// Function used to select the command to execute
void SendCommand(int dc_L, int dc_R) {
  static Motor L_motor(mL_en, mL_A, mL_B);
  static Motor R_motor(mR_en, mR_A, mR_B);
  //Serial.println(Command);
  // Motors commands
  if (Command == "q") {
    // Veicle turn Left
    turnTimeR = millis();
    controlStartL = false;
    controlStartR = true;
    motRotR = true;
    L_motor.stopMot();
    R_motor.stopMot();
    delay(1);
    //R_motor.forward(dc_R);
    R_motor.forward(dc_max); // turn left the vehicle trought right wheel
    //L_motor.forward(dc_min); // this line can be commented on
    Command = "";
  }
  else if (Command == "z") {
    // Veicle turn Left
    controlStartL = false;
    controlStartR = false;
    L_motor.back(dc_L);     // turn left the vehicle trought left wheel
    Command = "";
  }
  else if (Command == "a") {
    // Stop Left wheel
    controlStartL = false;
    controlStartR = false;
    L_motor.stopMot();
    Command = "";
  }
  else if (Command == "e") {
    // Veicle turn Right
    turnTimeL = millis();
    controlStartR = false;
    controlStartL = true;
    motRotL = true;
    R_motor.stopMot();
    L_motor.stopMot();
    delay(1);
    //L_motor.forward(dc_L);
    L_motor.forward(dc_max);  // turn right the vehicle trought left wheel
    //R_motor.forward(dc_min);  // this line can be commented on
    Command = "";
  }
  else if (Command == "c") {
    // Veicle turn Right
    controlStartL = false;
    controlStartR = false;
    R_motor.back(dc_R);       // turn right the vehicle trought right wheel
    Command = "";
  }
  else if (Command == "d") {
    // Stop Rigth wheel
    controlStartL = false;
    controlStartR = false;
    R_motor.stopMot();
    Command = "";
  }
  else if (Command == "s") {
    // Stop vehicle
    controlStartL = false;
    controlStartR = false;
    motFon = false;
    motBon = false;
    L_motor.stopMot();
    R_motor.stopMot();
    Command = "";
  }
  else if (Command == "w") {
    // Vehicle Forward
    controlStartL = true;
    controlStartR = true;
    motFon = true;
    motBon = false;
    L_motor.forward(dc_L);
    R_motor.forward(dc_R);
    Command = "";
  }
  else if (Command == "x") {
    // Vehicle Back
    controlStartL = true;
    controlStartR = true;
    motBon = true;
    motFon = false;
    L_motor.back(dc_L);
    R_motor.back(dc_R);
    Command = "";
  }
}
//------------------------------------------------
void setup() {
  Serial.begin(115200);
  // Serial for wi-fi communaication
  Serial1.begin (115200);

  // Attach motors pins
  analogWriteResolution(12);
  pinMode(mL_en, OUTPUT);
  pinMode(mR_en, OUTPUT);
  pinMode(mL_A, OUTPUT);
  pinMode(mL_B, OUTPUT);
  pinMode(mR_A, OUTPUT);
  pinMode(mR_B, OUTPUT);
  // Attach Encoder pins and interrupts
  pinMode(enL_sen, INPUT_PULLUP);
  pinMode(enR_sen, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(enL_sen), IncLc, RISING);
  attachInterrupt(digitalPinToInterrupt(enR_sen), IncRc, RISING);

  t = 0;
  cL = 0;
  cR = 0;
  rpsL = 0;
  rpsR = 0;
  dcL = 0;
  dcR = 0;

  Buffer = "";
  Data = "";
  Command = "";
  BufferGPS = "";

  start = false;
  wifiCon = false;
  okfound = false;
  readyfound = false;
  motFon = false;
  motBon = false;
  controlStartL = false;
  controlStartR = false;
  motRotR = false;
  motRotL = false;
  GPSstart = false;
  GPSfix = false;
}
//------------------------------------------------
void loop() {
}
// Function attached to interrupt of left wheel
void IncLc()
{
  cL++;
}
// Function attached to interrupt of right wheel
void IncRc()
{
  cR++;
}
//------------------------------------------------
// TASKS:
// T1: Control left wheel
void loop1(100) {
  static int k = 0;
  const int Ns = 10;     // number of samplings
  const int dcMax = 4096;// Max duty cycle
  const int dcMin = 1000;// Min duty cycle
  const int dcNom = 3600;// Nominal duty cycle
  const double v_ref = 1.5;// Velocity of reference
  const double KpL = 150;// K proportional
  const double KiL = 80;// K integrative
  //static int dcL = 0;
  static unsigned long tk[Ns];  // Time samples
  static unsigned long ckL[Ns]; // Rises detetcted from encoder
  static double vL[Ns];         // Velocity
  static double eL[Ns];         // Error
  static double eiL = 0.0;      // Integral of error
  static double vsL = 0.0;      // Sum of velocity
  static double vmL = 0.0;      // Mean velocity
  static unsigned long cRotL = 0;// Used to control the number of rotation of wheel
  static bool firstStartL = true;
  static bool rotLstart = false;
  static unsigned long AcL;   // Rise variation
  static unsigned long AcT;   // Time variation
  //------------------------------------------------
  tk[k] = millis();
  ckL[k] = cL;
  cL = 0;
  if (firstStartL) {
    vL[0] = 0;
    eL[0] = 0;
    vmL = 0;
    AcT = 0;
    AcL = 0;
    k = 1;
    firstStartL = false;
    //controlStartL = true;
  }
  else {
    if (k == 0) {
      // Compute changes ad time variation
      AcT = tk[k] - tk[Ns - 1];
      AcL = ckL[k];
      vL[k] = vL[Ns - 1];
      vsL = vL[k];
      // Compute Error
      //eL[k] = v_ref - vL[k];
      eL[k] = v_ref - vmL;
      // Compute integral of Error
      eiL = (eL[k] + eL[Ns - 1]) * (AcT / 2);
    }
    else {
      // Compute changes ad time variation
      AcT = tk[k] - tk[k - 1];
      AcL = ckL[k];
      // Compute velocity of wheels
      vL[k] = 50.0 * double(AcL) / double(AcT);
      // Compute mean velocity of wheels
      vsL += vL[k];
      vmL = vsL / double(k + 1);
      // Compute Error
      //eL[k] = v_ref - vL[k];
      eL[k] = v_ref - vmL;
      // Compute integral of Error
      eiL = (eL[k] + eL[k - 1]) * (AcT / 2);
    }
    // compute Duty Cycle
    //Serial.println(String(k)+" : "+String(vL[k]));
    dcL = dcNom + int(KpL * eL[k]) + int(KiL * eiL);
    // Limit DC left wheel
    if (dcL > dcMax) {
      dcL = dcMax;
    }
    if (dcL < dcMin) {
      dcL = dcMin;
    }
    if (controlStartL) {
      // Send control command when control start
      if (motRotL) {
        //cRotL += AcL;
        //Serial.println(cRotL);
        //if (cRotL >= 10) {
        if (tk[k] - turnTimeL >= 500) {
          cRotL = 0;
          motRotL = false;
          if (motFon) {
            Command = "w";
          } else if (motBon) {
            Command = "x";
          } else {
            Command = "s";
          }
        }
        SendCommand(dcL, dcR);
      }
      else if (motFon) {
        Command = "w";
        SendCommand(dcL, dcR);
      }
      else if (motBon) {
        Command = "x";
        SendCommand(dcL, dcR);
      }
    }
    k++;
    if (k == Ns) {
      rpsL = vmL; // store data
      //cL = 0;
      //vmL = 0;
      k = 0;
    }
  }
}

// T2: Control right wheel
void loop2(100) {
  static int k = 0;
  const int Ns = 10;     // number of samplings
  const int dcMax = 4096;// Max duty cycle
  const int dcMin = 1000;// Min duty cycle
  const int dcNom = 3600;// Nominal duty cycle
  const double v_ref = 1.5;// Velocity of reference
  const double KpR = 150;// K proportional
  const double KiR = 80;// K integrative
  //static int dcR = 0;
  static unsigned long tk[Ns];
  static unsigned long ckR[Ns]; // R trans for sampling
  static double vR[Ns];         // Velocity
  static double eR[Ns];         // Error
  static double eiR = 0.0;      // Integral of error
  static double vsR = 0.0;
  static double vmR = 0.0;
  static unsigned long cRotR = 0;// Used to control the number of rotation of wheel
  static bool firstStartR = true;
  static bool rotRstart = false;
  static unsigned long AcR;   // Rise variation
  static unsigned long AcT;   // Time variation
  //------------------------------------------------
  tk[k] = millis();
  ckR[k] = cR;
  cR = 0;
  if (firstStartR) {
    vR[0] = 0;
    eR[0] = 0;
    vmR = 0;
    AcT = 0;
    AcR = 0;
    k = 1;
    firstStartR = false;
  }
  else {
    if (k == 0) {
      // Compute changes ad time variation
      AcT = tk[k] - tk[Ns - 1];
      AcR = ckR[k];
      vR[k] = vR[Ns - 1];
      vsR = vR[k];
      // Compute Error
      //eR[k] = v_ref - vR[k];
      eR[k] = v_ref - vmR;
      // Compute integral ofError
      eiR = (eR[k] + eR[Ns - 1]) * (AcT / 2);
    }
    else {
      // Compute changes ad time variation
      AcT = tk[k] - tk[k - 1];
      AcR = ckR[k];
      // Compute velocity of wheels
      vR[k] = 50.0 * double(AcR) / double(AcT);
      // Compute mean velocity of wheels
      vsR += vR[k];
      vmR = vsR / double(k + 1);
      // Compute Error
      //eR[k] = v_ref - vR[k];
      eR[k] = v_ref - vmR;
      // Compute integral ofError
      eiR = (eR[k] + eR[k - 1]) * (AcT / 2);
    }
    // Compute Duty Cycle
    dcR = dcNom + int(KpR * eR[k]) + int(KiR * eiR);
    // Limit DC right wheel
    if (dcR > dcMax) {
      dcR = dcMax;
    }
    if (dcR < dcMin) {
      dcR = dcMin;
    }
    if (controlStartR) {
      // Send control command
      if (motRotR) {
        //cRotR += AcR;
        //Serial.println(cRotL);
        //if (cRotR >= 10) {
        if (tk[k] - turnTimeR >= 500) {
          cRotR = 0;
          motRotR = false;
          if (motFon) {
            Command = "w";
          } else if (motBon) {
            Command = "x";
          } else {
            Command = "s";
          }
        }
        SendCommand(dcL, dcR);
      }
      else if (motFon) {
        Command = "w";
        SendCommand(dcL, dcR);
      }
      else if (motBon) {
        Command = "x";
        SendCommand(dcL, dcR);
      }
    }
    k++;
    if (k == Ns) {
      //rpsR = float(ckR[Ns - 1]) / CFL;
      rpsR = vmR; // store data
      //cR = 0;
      //vmR=0;
      k = 0;
    }
  }
}
// T3: Test PC serial and W-Fi serial
void loop3(50) {
  static int indx = 0;
  // Test if serial command is available
  if (Serial.available() > 0) {
    Command = Serial.readString();
    SendCommand(dc_max, dc_max);
  }
  // Test if serial WiFi is available
  while (Serial1.available () > 0) {
    Buffer = Serial1.readStringUntil('\r');
    Serial.print(Buffer);
    if ( Buffer.lastIndexOf("+IPD") >= 0 ) {
      indx = Buffer.lastIndexOf(":");
      Command = Buffer.substring(indx + 1);
      //Serial.print("Wi-Fi command: ");
      //Serial.print(Command);
      SendCommand(dc_max, dc_max);
      Data = "ok";
      Buffer = "";
    }
    else if (Buffer.lastIndexOf("Link") >= 0) {
      //Serial.println("");
      //Serial.println("Connected");
      Data = "connected";
      wifiCon = true;
      Buffer = "";
    }
    else if (Buffer.lastIndexOf("Unlink") >= 0) {
      //Serial.println("");
      //Serial.println("Disconnected");
      //Serial1.flush();
      Data = "";
      wifiCon = false;
      Buffer = "";
    }
    else if ( Buffer.lastIndexOf("OK") >= 0 ) {
      okfound = true;
      Buffer = "";
    }
    else if ( Buffer.lastIndexOf("ready") >= 0 ) {
      readyfound = true;
      Buffer = "";
    }
    //Buffer = "";
  }
}
// T4: Obstacle detecting
void loop4(200) {
  float cmM;
  static Ultrasonic ultrasonic(ul_trig, ul_eco);
  cmM = float(ultrasonic.distanceRead(CM));
  //Serial.print("Sensore di distanza [cm]: ");
  //Serial.println(cmMsec);
  if ( cmM <= 5 ) {
    Command = "s";
    SendCommand(0, 0);
  }
}
// T5: WiFi handle
void loop5(500) {
  static WiFiESP8266 wifi(Network, Password, KeyType, Channel);
  if (!start) {
    // WiFi initialize
    wifi.init();
    delay(100);
    wifi.SetSoftAPmode();
    delay(100);
    wifi.CreateServer();
    delay(100);
    Serial.println("START WiFi OK");
    start = true;
  } else {
    if (Data != "") {
      if ( !wifiCon && start) {
        Serial.println(Data);
        //Serial.println(datastr);
        Data = "";
      }
      else if ( wifiCon) { //&& Serial1.available () <= 0) {
        while (Serial1.availableForWrite() <= 0);
        // WiFi send data
        wifi.SendDataRequest(Data);
        //Serial1.flush();
        Data = "";
      }
      //Data = "";
    }
  }
  // WiFi command info
  if (Command == "1") {
    wifi.GetIP_ClientPC();
    Command = "";
  }
}
// T6: Data to send creation and GPS handler
void loop6(500) {
  static String datastr = "";
  static float lat = 0;
  static float lon = 0;
  // Connect to the GPS on the hardware port
  static Adafruit_GPS GPS(&GPSSerial);
  t = millis();
  if (start) {
    if (!GPSstart ) {
      GPS.begin(9600);
      GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
      while (GPSSerial.available())
        GPS.read();
      Serial.print("\nSTARTING LOGGING....");
      if (GPS.LOCUS_StartLogger())
        Serial.println(" STARTED! ");
      else
        Serial.println(" no response :(");
      GPSstart = true;
    } else {
      while (GPSSerial.available () > 0) {
        BufferGPS += GPS.read();
      }
      if (GPS.newNMEAreceived()) {
        //Serial.println(GPS.lastNMEA()); // sets the newNMEAreceived() flag to false
        if (!GPS.parse(GPS.lastNMEA())) // sets the newNMEAreceived() flag to false
          return; // we can fail to parse a sentence in which case we should just wait for another
        lat = GPS.latitudeDegrees;
        lon = GPS.longitudeDegrees;
        GPSfix = GPS.fix;
      }
      BufferGPS = "";
    }
    if (Data == "") {
      if (!wifiCon) {
        datastr = String(t);
        datastr += ",";
        datastr += String(rpsL);
        datastr += ",";
        datastr += String(rpsR);
        Data = datastr;
      }
      else {
        if (GPSfix) {
          datastr = String(t);
          datastr += ":";
          datastr += String(lat, 6);
          datastr += ",";
          datastr += String(lon, 6);
          Data = datastr;
        } else {
          Data = "connected";
        }
      }
    }
  }
}