Arduino101 BLE Autonomous Rover

/* Authors: Oliver Chen, Raad Hashim, Prashant Lalwani and Purval Sule
   
   Autonomous Arduino 101 based 4WD Rover code.
   This code drives an Arduino101 microcontroller board
   using the builtin Bluetooth Low Energy (BLE) functonality
   , 4 sets of motor on the SEEDstudio Skeleton Bot chassis
   driven by 2 grove I2C motor control drivers.
   Additional components included from SEEDstudio include:
   Grove LED Socket, Green LED, Grove 80cm proximity sensor

   Motor driver example code found at:
   https://github.com/Seeed-Studio/Grove_I2C_Motor_Driver

   Initial UART BLE from Dave Shade's Arduino101 BLE Rover

   This demo code is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
 
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
 
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 

*/

// Tested on Ubuntu 16.04, Arduino 1.6.13, and corelibs 1.0.7

////////////////
// Libraries //
//////////////

// Curie Bluetooth Library
#include <CurieBLE.h>
// I2C Library
#include <Wire.h>

//////////////
// Defines //
////////////

// defines required by motor driver functions
#define MotorSpeedSet             0x82
#define PWMFrequenceSet           0x84
#define DirectionSet              0xaa
#define MotorSetA                 0xa1
#define MotorSetB                 0xa5
#define Nothing                   0x01
// Note: each I2C motor driver has a unique address (selectable on board)
#define I2CMotorDriver_right_Addr 0x01   // Set the address of the I2CMotorDriver - right
#define I2CMotorDriver_left_Addr  0x02   // Set the address of the I2CMotorDriver - left
#define IR_PROXIMITY_SENSOR A1 // Analog input pin that  is attached to the sensor
#define ADC_REF 5//reference voltage of ADC is 5v.If the Vcc switch on the Seeeduino
                     //board switches to 3V3, the ADC_REF should be 3.3
float voltage;//the sensor voltage, you can calculate or find the distance
                    // to the reflective object according to the figures
                    //on page 4 or page 5 of the datasheet of the GP2Y0A21YK.


///////////////
// Pin list //
/////////////

const int connect_led_pin = 2;  // pin used for connect status LED. Doubles up as Calibration failed

//////////////////////////////////////
// Motor Speed  and direction task //
////////////////////////////////////

// Function to set Motor A and B speed as well as direction for a specific motor driver (address)
// Note: 1001 indicates M1 in forward and M2 in reverse direction
// Note: Bit 0 and 1 should be a complement and Bit 1 and 2 should be complement
void Set_MotorSpeed_and_direction(unsigned char MotorSpeedA, unsigned char MotorSpeedB, unsigned char Direction, unsigned char I2C_MotorDriver_Addr)
{
  //Convert 0-100% to analog values 0-255
  MotorSpeedA = map(MotorSpeedA, 0, 100, 0, 255);
  MotorSpeedB = map(MotorSpeedB, 0, 100, 0, 255);

  // Speed
  Wire.beginTransmission(I2C_MotorDriver_Addr); // transmit to specified address
  Wire.write(MotorSpeedSet);                    // set pwm header
  Wire.write(MotorSpeedA);                      // send pwma
  Wire.write(MotorSpeedB);                      // send pwmb
  Wire.endTransmission();               	      // stop transmitting

  // Direction
  Wire.beginTransmission(I2C_MotorDriver_Addr); // transmit to device I2CMotorDriverAdd
  Wire.write(DirectionSet);                     // Direction control header
  Wire.write(Direction);                        // send direction control information
  Wire.write(Nothing);                          // need to send this byte as the third byte(no meaning)
  Wire.endTransmission();                       // stop transmitting
}

///////////////////////
// Global Variables //
/////////////////////
String cmdInput; // Command received from BLE- Auto:Stop, Auto:Up, Auto:Right, Auto:Left, Auto:Back, Auto:Auto
String lastCmd="";
int lastTurn=0;
int turnTime=1000;

/////////////////////////////////
// BLE handle and definitions //
///////////////////////////////

BLEPeripheral blePeripheral;
BLEService uartService = BLEService("6E400001B5A3F393E0A9E50E24DCCA9E");
// create characteristics
BLECharacteristic rxCharacteristic = BLECharacteristic("6E400002B5A3F393E0A9E50E24DCCA9E", BLEWriteWithoutResponse, 20);  // == TX on central (central control app)
BLECharacteristic txCharacteristic = BLECharacteristic("6E400003B5A3F393E0A9E50E24DCCA9E", BLENotify , 20); // == RX on central (central control app)

bool connectionStatus = false;

void roverStop(){
  // Turn off all Motors (to be safe)
  Set_MotorSpeed_and_direction(0, 0, 0b1010, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(0, 0, 0b0101, I2CMotorDriver_left_Addr);
  Serial.println("Rover Stop");
}

void roverUp(){
  Set_MotorSpeed_and_direction(40, 40, 0b1010, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(40, 40, 0b0101, I2CMotorDriver_left_Addr);
  Serial.println("Rover Up");
}

void roverBack(){
  Set_MotorSpeed_and_direction(40, 40, 0b0101, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(40, 40, 0b1010, I2CMotorDriver_left_Addr);
  Serial.println("Rover Up");
}

void roverLeft(){
  Set_MotorSpeed_and_direction(100, 100, 0b1010, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(1, 1, 0b0101, I2CMotorDriver_left_Addr);
  Serial.println("Rover Left");
}

void roverRight(){
  Set_MotorSpeed_and_direction(1, 1, 0b1010, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(100, 100, 0b0101, I2CMotorDriver_left_Addr);
  Serial.println("Rover Right");
}

void roverAvoidLeft(){
  Set_MotorSpeed_and_direction(100, 100, 0b1010, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(50, 50, 0b1010, I2CMotorDriver_left_Addr);
  Serial.println("Rover Avoid Left");
  delay(turnTime);
}

void roverAvoidRight(){
  Set_MotorSpeed_and_direction(50, 50, 0b0101, I2CMotorDriver_right_Addr);
  Set_MotorSpeed_and_direction(100, 100, 0b0101, I2CMotorDriver_left_Addr);
  Serial.println("Rover Avoid Right");
  delay(turnTime);
}

void roverAuto(){
  lastCmd=cmdInput;
  Serial.println("Rover Auto");
  if (voltage < 1) {
    roverUp();
  } else if (lastTurn==0) {
    roverAvoidRight();
    lastTurn=1;

  } else {
    roverAvoidLeft();
    lastTurn=0;
  }

}
void fullStop(){
  roverStop();
}

/////////////////
// Setup Loop //
///////////////

void setup()
{
  // Setting up serial connection
  Serial.begin(9600);

  // join i2c bus (address optional for master)
  Wire.begin();

  // wait to make sure I2C is initialized
  delayMicroseconds(10000);

  // specifying connection LED pin as output
  pinMode(connect_led_pin, OUTPUT);

  blePeripheral.setLocalName("4WDUART");
  blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
  // add service, rx and tx characteristics:
  blePeripheral.addAttribute(uartService);
  blePeripheral.addAttribute(rxCharacteristic);
  blePeripheral.addAttribute(txCharacteristic);
  // assign event handlers for connected, disconnected to peripheral
  blePeripheral.setEventHandler(BLEConnected, blePeripheralConnectHandler);
  blePeripheral.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);
  rxCharacteristic.setEventHandler(BLEWritten, rxCharacteristicWritten);
  blePeripheral.begin();
  delay(5000);
  Serial.println("BLE UART Peripheral");
}

////////////////
// Main loop //
//////////////

void loop()
{
  // Keep polling over the Peripheral
  blePeripheral.poll();

  // Check BLE connection before executing any code
  if (connectionStatus)
  {
    // Turn on connection LED
    digitalWrite(connect_led_pin, HIGH);
   
    Serial.print(String(cmdInput));
    voltage = getVoltage();
    Serial.print(" sensor voltage  = " );                       
    Serial.println(voltage);

    if (String(cmdInput) == "Auto:Stop") {    
        roverStop();
    } else if ((String(cmdInput) == "Auto:Auto") or (String(lastCmd) == "Auto:Auto")){
        roverAuto();
    } else if (String(cmdInput) == "Auto:Up"){
        roverUp();
    } else if (String(cmdInput) == "Auto:Left"){
        roverLeft();
    } else if (String(cmdInput) == "Auto:Right"){
        roverRight();
    } else if (String(cmdInput) == "Auto:Back"){
        roverBack();
    } else {
        fullStop();
    }
    lastCmd=cmdInput;
  }
  else
  {
     Serial.println("not connected");
    // Turn off connection LED
    digitalWrite(connect_led_pin, LOW);
    
    // Turn off everything to be safe
    fullStop();
  }
} // void loop()

void blePeripheralConnectHandler(BLECentral& central) {
  // central connected event handler
  Serial.print("Connected event, central: ");
  Serial.println(central.address());
  digitalWrite(connect_led_pin, HIGH);
  connectionStatus = true;
}

void blePeripheralDisconnectHandler(BLECentral& central) {
  // central disconnected event handler
  Serial.print("Disconnected event, central: ");
  Serial.println(central.address());
  digitalWrite(connect_led_pin, LOW);
  connectionStatus = false;
}

void rxCharacteristicWritten(BLECentral& central, BLECharacteristic& characteristic) {
  // central wrote new value to characteristic, update LED
  Serial.print("Characteristic event, written: ");
  if (characteristic.value()) {       //null pointer check
    int len = characteristic.valueLength(); //get size 
    if (cmdInput) {
      lastCmd=cmdInput;
    }
    cmdInput= "";
    for(int i=0; i<len; i++){
      cmdInput+= (char)*(characteristic.value()+i);
    }
    Serial.println(cmdInput);
  } 
}

//////////////////////////////////////////////////////////
//Get voltage from the sensor connected to analog pin A0//
//Parameter:-void                                       //
//Return:   -float,the voltage of the analog pin        //
//////////////////////////////////////////////////////////
float getVoltage()
{
    int sensor_value;
    int sum;  
    // read the analog in value:
    for (int i = 0;i < 20;i ++)//Continuous sampling 20 times
    {
        sensor_value = analogRead(IR_PROXIMITY_SENSOR); 
        sum += sensor_value;
    }
    sensor_value = sum / 20;
    float voltage;
    voltage = (float)sensor_value*ADC_REF/1024;
    return voltage;
}