Arduino 101 BLE Rover

/*
 * Arduino 101 based 4WD Rover code developed utilizing:  
 * - an Arduino 101 microcontroller board - using the Bluetooth LE radio (BLE), and the 6-axis Inertial Measurement Unit (IMU)
 * - motors and chassis from Skeleton Bot - 4WD hercules mobile robotic platform from Seeedstudio - early version 
 * - Grove I2C Motor Driver Board
 * - Grove Starter Kit for Arduino from Seeedstudio - Grove Button, Grove I2C RGB LCD Display, Grove LED Socket, 
 * - HC-SR04 Ultrasonic sensor, wired via a small breadboard
 * 
 * Most of this project's code is derived from other Arduino samples found 
 * on www.instructables.com and www.github.com
 * 
 * Much respect and appreciation for the "Makers" who have come before me
 * Lots of knowledge gained from their code and hope my example here is useful to others
 * 
 * the integration and other bits of original code were written by:
 * Author: Dave Shade
 * 
 * it is available for use under the GNU Lesser General Public License
 * see below for details
 * 
 * *** Significant pieces of code used from the following: ***
 * 
 * Code included for the Grove I2C Motor Driver from:
 * Grove - I2C motor driver demo v1.0
 * by: http://www.seeedstudio.com
 *
 * Code for the RoverControl function taken from the Basic_Robt.ino Sketch:
 * http://www.instructables.com/id/Smartphone-Controlled-Arduino-Rover/
 * by deba168
 * 
 * Code for the Crash Detection derived from the Arduino 101 sample: ShockDetect
 *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
 * Code for the BlueTooth Remote Control derived from the Arduino 101 sample: CallbackLED
 *    Copyright (c) 2015 Intel Corporation.  All rights reserved.
 *    
 * Specifics for the BLE UART characteristics - to enable the use of the Nordic Semiconductor UART applicaion
 * found at: https://www.nordicsemi.com/eng/Products/Nordic-mobile-Apps/nRF-UART-App2
 * 
 * Code included from various Grove examples like: 
 * Grove RGB LCD display, Grove Button, Grove LED and they are:
 * 2013 Copyright (c) Seeed Technology Inc.  All right reserved.
 * 
 * All code utilized is covered under the license agreement described below: 
 *  
 *  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
 *
*/

#include <Wire.h>   // Library for the initializing the I2C bus.
#include "rgb_lcd.h"   // Grove library for the I2C RGB LCD Display is found at: https://github.com/Seeed-Studio/Sketchbook_Starter_Kit_V2.0
#include "CurieIMU.h"   // CurieIMU library - pre-installed when Arduino 101 is selected in Arduino IDE 1.6.7 or later 
#include <CurieBLE.h>   // CurieBLE library - pre-installed when Arduino 101 is selected in Arduino IDE 1.6.7 or later 

//I2C Grove Motor Driver defines
//Motor controller defines
#define MotorSpeedSet             0x82
#define PWMFrequenceSet           0x84
#define DirectionSet              0xaa
#define MotorSetA                 0xa1
#define MotorSetB                 0xa5
#define Nothing                   0x01
#define EnableStepper             0x1a
#define UnenableStepper           0x1b
#define Stepernu                  0x1c
#define I2CMotorDriverAdd         0x0f // Set the address of the I2CMotorDriver

// Variables for allocating the Digital I/O pins of the Arduino
const int resetPin = 2;           // D2 used by Grove Button - reset from crash state
const int ledPin = 3;             // D3 used by Grove LED socket - showing bluetooth connected
//const int servoPin = 5;         // D5 used by the servo
//const int ultrasonic_back = 6;  // D6 used by the Back ultrasonic sensor
                                  // D7 used by the Back ultrsonic sensor
//const int ultrasonic_front = 8;   // D8 used by the Front ultrasonic sensor
                                  // D9 used by the Front ultrasonic sensor
// 0x0f is the I2C address used by Grove Motor Driver board
// 0x3e is an I2C address used by the Grove RGB LCD display (7c>>1)
// 0x62 is an I2C address used by the Grove RGB LCD display (c4>>1)

// Variables for the Grove RGB LCD Display - connected to the I2C bus
// common RGB values: Red=255,0,0  Green=0,255,0  Blue=0,0,255  Purple=255,0,255  Yellow=255,255,0  Aqua=0,255,255  White=255,255,255 Off=0,0,0
int colorR = 0;
int colorG = 255;
int colorB = 0;

char vSpeedSet = 50;    // this variable holds the speed setting from the remote control - default speed is half
char vSpeedLimit = 50;  // this variable holds the limited value of the speed based on sensor values

bool crash = false;  // initial crash state is false (operating)

char state = 'c';  // initial state is stop
char prev_state = 'c';

// Creating the instance of the LCD display
rgb_lcd lcd;

BLEPeripheral blePeripheral;  // BLE Peripheral Device (the board you're programming)
// ====  create Nordic Semiconductor BLE UART service =========
// Must use these UUIDs and BLE characteristics
BLEService uartService = BLEService("6E400001B5A3F393E0A9E50E24DCCA9E");
// create characteristics
BLECharacteristic rxCharacteristic = BLECharacteristic("6E400002B5A3F393E0A9E50E24DCCA9E", BLEWriteWithoutResponse, 20);   // == TX on central (android app)
BLECharacteristic txCharacteristic = BLECharacteristic("6E400003B5A3F393E0A9E50E24DCCA9E", BLENotify , 20);   // == RX on central (android app)
 

// Setup function - run once at reset or power-on
void setup()  {
  Wire.begin();   // join i2c bus (address optional for master)
  delay(100);
  Serial.begin(115200);
  // initialize the Reset button on D2 and the Blue Grove LED on D3
  pinMode(resetPin, INPUT);   //use the switch which has a pull-up resistor aand connects to ground
  pinMode(ledPin, OUTPUT);   // use the LED on pin 3 as an output

  // set advertised local name and service UUID:
  blePeripheral.setLocalName("4WD_RV");   //make unique name
  blePeripheral.setAdvertisedServiceUuid(uartService.uuid());

  // add service and characteristic:
  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);

  // assign event handler for characteristic
  rxCharacteristic.setEventHandler(BLEWritten, rxCharacteristicWritten);

  // begin advertising BLE service:
  blePeripheral.begin();
  //Serial.println(("Bluetooth device active, waiting for connections..."));  // for debugging
  
 /* Initialise the IMU and create an Interrupt service routine to deal with a shock - indicating a crash
  * Shutting off the electric motors after a big shock can help to minimize damage to rover motors and circuitry
  */
    
  CurieIMU.begin();
  CurieIMU.attachInterrupt(CrashEventCallback);   // Enable Shock Detection 
 /* 
  * The Threshold value should be adjusted through some experimentation 
  * so that crashes are detected at reasonable shock levels
  * this is to ensure the safety of the operator and to minimize stress on the motors
  * typical values for the function are from 1500 - 2000
  * there were updates to the Arduino 101 firmware and CurieIMU library in July 2016
  * please ensure you have the latest version using the board manager of the Arduino IDE
  * 
  */
  CurieIMU.setDetectionThreshold(CURIE_IMU_SHOCK, 1900);   // 2.0g = 2000mg; 1.5g = 1500mg, etc.
  CurieIMU.setDetectionDuration(CURIE_IMU_SHOCK, 50);   // 50ms
  CurieIMU.interrupts(CURIE_IMU_SHOCK);
  //Serial.println("IMU initialization complete, waiting for events...");   // for debugging

  //Serial.println("LCD setup begin");    // for debugging
  // set up the grove RGB LCD's number of columns and rows:
  lcd.begin(16, 2);
  lcd.setRGB(colorR, colorG, colorB);  

  // Print message headers to the LCD.
  lcd.setCursor(0,0);  // cursor to home position - it starts there...
  lcd.print("motor speed % =");
  //lcd.setCursor(0, 1);
  //lcd.print("speed  = ");

  // Initialize the motor controllers  
  MotorDirectionSet(0b0000);  //0b0000  stopped 
  delay(100);
  MotorSpeedSetAB(vSpeedSet,vSpeedSet); // on a scale of 1 to 100
  delay(100);  
}  // end setup

void loop()  {
 /* Key sections of the loop
  *  Check if we are in a "crash" condition
  *  if not crashed:
  *    poll for a new value written into state by the rxCharacteristic
  *    check if there is something new in "state"  
  *      if yes call rovercontrol
  *      if no call speedcontrol
  *  if crashed: 
  *    look for a reset button press
  */
  if (crash == false) {   //rover is operating normally
    blePeripheral.poll();
    if(state != prev_state) {   // check if the value from the remote control new or the same as the previous one?
      RoverControl(state);   // function to respond to control state changes - direction and/or speed
      //Serial.println(char(state));
      prev_state = state;
    }
  }
  else {       //rover is in a crashed state
    if (digitalRead(resetPin) == HIGH)  {   // look for reset condition - this occurs from a push button after a crash
      CrashRecovery();  // function to recover from crash, start operating normally again
    }
  }
}

void blePeripheralConnectHandler(BLECentral& central) {   // central connected event handler
  Serial.print("Connected event, central: ");
  Serial.println(central.address());
  digitalWrite(ledPin, HIGH);     
  //Serial.println("LED on");
}

void blePeripheralDisconnectHandler(BLECentral& central) {   // central disconnected event handler
  Serial.print("Disconnected event, central: ");
  Serial.println(central.address());
  digitalWrite(ledPin, LOW);      
  //Serial.println("LED off");
  state = 'c';
  RoverControl(state);            // stop rover on BLE disconnect - should reduce runaway rover incidents
  //Serial.println(char(state));
  delay(100);
}

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
    state = *characteristic.value();   // de-reference to get first byte
    //Serial.println(char(state));
  } 
}

// Functions to set the 2 DC motor's speed: motorSpeedA: the DC motor A speed; should be 0~100, motorSpeedB: the DC motor B speed; should be 0~100;
void MotorSpeedSetAB(unsigned char MotorSpeedA , unsigned char MotorSpeedB)  {
  MotorSpeedA=map(MotorSpeedA,0,100,0,255);
  MotorSpeedB=map(MotorSpeedB,0,100,0,255);
  Wire.beginTransmission(I2CMotorDriverAdd); // transmit to device I2CMotorDriverAdd
  Wire.write(MotorSpeedSet);   // set pwm header 
  Wire.write(MotorSpeedA);     // send pwma 
  Wire.write(MotorSpeedB);     // send pwmb    
  Wire.endTransmission();    // stop transmitting
}

// set the direction of DC motor. 
void MotorDirectionSet(unsigned char Direction)  {   //  Adjust the direction of the motors 0b0000 I4 I3 I2 I1
  Wire.beginTransmission(I2CMotorDriverAdd);   // 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 
}

void RoverControl(char state) {
 /*
  * Respond to changes in direction - forward, left, stop, right, backward from Bluetooth LE Remote Control
  *
  * With a new "state" from the remote control - check for action to take
  * a - set gear to go forward (drive)
  * b - set gear to turn left and go forward
  * c - set gear to stop (park)
  * d - set gear to turn right and go forward
  * e - set gear to go backward (reverse)
  * 0 - set speed to 0 - not implemented in the remote control
  * 1 - set speed to 25%
  * 2 - set speed to 50%
  * 3 - set speed to 75% 
  * 4 - set speed to 100%
  */
  if (state == 'a') {
    MotorDirectionSet(0b1001); }  //0b1001    // If state is equal with letter 'a', Motors Rotating in the forward direction, rover will go forward 
  else if (state == 'b') {
    MotorDirectionSet(0b0001); }  //0b0001    // If state is equal with letter 'b', right motors rotating forward, left motors off, , rover will turn left
  else if (state == 'd') {
    MotorDirectionSet(0b1000); }  //0b1000    // If state is equal with letter 'd', left motors rotating forward, right motors off, rover will turn right
  else if (state == 'c'){
    MotorDirectionSet(0b0000); }  //0b0000    // If state is equal with letter 'c', Motors off - stop the rover
  else if (state == 'e') {
    MotorDirectionSet(0b0110); }  //0b0110    // If state is equal with letter 'e', rover will go backward, both motors rotating backward
  else if (state == '0') {   // Change speed if state is equal from 0 to 4. Values must be from 0 to 100 - this is mapped to 0 to 255 (PWM) by the MotorSpeedSet function
    vSpeedSet=0; }
  else if (state == '1') {
    vSpeedSet=25; }
  else if (state == '2') {
    vSpeedSet=50; }
  else if (state == '3') {
    vSpeedSet=75; }
  else if (state == '4') {
    vSpeedSet=100; }
  delay(10);
  vSpeedLimit = vSpeedSet;
  MotorSpeedSetAB(vSpeedSet,vSpeedSet);   // set motor speed based on changes on a scale of 1 to 100
  lcd.setCursor(0, 1);
  lcd.print("   ");
  lcd.setCursor(0, 1);
  lcd.print(vSpeedSet);
  delay(10);
}


// function to reset the Rover after a crash - called when the reset push button is pushed
void CrashRecovery(void)  {
  MotorDirectionSet(0b0000);   //0b0000  motor stop
  vSpeedSet = 50;
  vSpeedLimit = vSpeedSet;
  MotorSpeedSetAB(vSpeedSet,vSpeedSet);   // restore motor speed to previous value - on a scale of 1 to 100
  // restore the LCD display to its precrash values
  lcd.setRGB(0,255,0);   // make backlight green
  lcd.setCursor (0,0);
  lcd.clear();
  lcd.print("motor speed % =");
  lcd.setCursor(0, 1);
  lcd.print(vSpeedSet);
  delay(10);
  crash = false;
  state = 'c';
}

// Callback function from the CurieIMU ShockDetect Sample
static void CrashEventCallback(void) {
  if (CurieIMU.getInterruptStatus(CURIE_IMU_SHOCK)) {
    // Here is where to define what to do in crash situation
    // should definitely turn off the motors for safety
    MotorDirectionSet(0b0000);  //0b0000  stop motors
    delay(10);
    MotorSpeedSetAB(50,50);  // set speed to 50 on a scale of 1 to 100
    lcd.setRGB(255,0,0);   // make backlight red
    lcd.setCursor(0,0);   // reset cursor to upper left
    lcd.clear();   // clear display
    lcd.println("CRASH!!!!!!");   // write crash message to lcd
    delay(10);
    state = 'c';
    prev_state = 'c'; 
    crash = true;
  }
}