Yulee Rover

/*************************************************************
  @author: Jerico Cruz
 */
#define BLYNK_PRINT Serial
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
#include <LiquidCrystal_I2C.h>
#include <NewPing.h>
LiquidCrystal_I2C lcd(0x27,16,2);

#define TRIGGER_PIN 2 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 3 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

//MotorShield
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
Adafruit_DCMotor *frontMotor = AFMS.getMotor(1);
Adafruit_DCMotor *rearMotor = AFMS.getMotor(2);
char command;
unsigned int speed;

//Bluetooth
#include <SoftwareSerial.h>
SoftwareSerial SwSerial(10, 11); // RX, TX
    
#include <BlynkSimpleSerialBLE.h>

// You should get Auth Token in the Blynk App.
// Go to the Project Settings (nut icon).
char auth[] = "";

SoftwareSerial SerialBLE(10, 11); // RX, TX
//End Bluetooth

//Sonar
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
//End Sonar

//Virtual pin methods from BLYNK
BLYNK_WRITE(V0)
{
  speed = param.asInt(); // assigning incoming value from pin V1 to a variable
  // You can also use:
  // String i = param.asStr();
  // double d = param.asDouble();
}

BLYNK_WRITE(V1)
{
  int x = param[0].asInt(); // assigning incoming value from pin V1 to a variable
  int y = param[1].asInt();
  // You can also use:
  // String i = param.asStr();
  // double d = param.asDouble();
  
  //Determine motor commands through x and y axis values.
  if(x == 1 && y == 2)
    command = 'F';
  else if(x == y)
    command = 'S';
  else if (x == 2 && y == 1)
    command = 'L';
  else if(x == 0 && y == 1)
    command = 'R';
  else if(x == 1 && y == 0)
    command = 'B';
}

BLYNK_WRITE(V2)
{
   if(param.asInt()){
     readDistance();
   } 
  
}
//End virtual pin method from BLYNK

void setup()
{
  // Debug console
  Serial.begin(9600);

  SerialBLE.begin(9600);
  Blynk.begin(SerialBLE, auth);

  Serial.println("Waiting for connections...");
  
  AFMS.begin();  // create with the default frequency 1.6KHz
  //AFMS.begin(1000);  // OR with a different frequency, say 1KHz
  
  // Set initial motor speed of 100 MAX is 255
  speed = 100;
  
  //Initialize motors
  frontMotor->setSpeed(speed); 
  rearMotor->setSpeed(speed); 
  frontMotor->run(FORWARD);
  rearMotor->run(FORWARD);
  
  delay(1000);
  
  frontMotor->run(RELEASE); 
  rearMotor->run(RELEASE); 
  Blynk.virtualWrite(V0, speed);
  
  //Start LCD
  lcd.init(); 
  lcd.backlight();
}

void loop()
{
  //readDistance();
  Blynk.run();
  halt();
  switch(command){
    case 'F':  
      forward();
      break;
    case 'B':  
     reverse();
      break;
    case 'L':  
      left();
      break;
    case 'R':
      right();
      break;
    case 'S':
      halt();
      break;
  }
}

void readDistance(){
  delay(100); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int uS = sonar.ping_cm(); // Send ping, get ping in centimeters
  Serial.println(uS);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Distance:");
  lcd.setCursor(10, 0);
  lcd.print(uS);
  lcd.setCursor(13, 0);
  lcd.print("cm");
  
  lcd.setCursor(0, 1);
  lcd.print(" ");
  
}

void halt(){
  //Serial.println("Stop");
  frontMotor->setSpeed(0); 
  frontMotor->run(RELEASE);
  rearMotor->setSpeed(0);
  rearMotor->run(RELEASE);
}

void forward(){
  Serial.println(command);
  frontMotor->setSpeed(speed);
  frontMotor->run(FORWARD);
  rearMotor->setSpeed(speed);
  rearMotor->run(FORWARD);
}

void reverse(){
  Serial.println(command);
  frontMotor->setSpeed(speed);
  frontMotor->run(BACKWARD);
  rearMotor->setSpeed(speed); 
  rearMotor->run(BACKWARD);
}

void right(){
  Serial.println(command);
  frontMotor->setSpeed(speed);
  frontMotor->run(FORWARD);
  rearMotor->setSpeed(0); 
  rearMotor->run(RELEASE);
}

void left(){
  Serial.println(command);
  frontMotor->setSpeed(0);
  frontMotor->run(RELEASE);
  rearMotor->setSpeed(speed); 
  rearMotor->run(FORWARD);
}