BCNLABS2 - BLE BOT 9000 with Intel Arduino 101

/* BLEBOOT 9000 BCN LABS          */
/* MAY 2017 - Mobile World Center */

/* First Draw, todo : review code on next LAB :: PepLluis 12/05/2017 :-) */

#include <CurieBLE.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

// On board led
const int ledPin = 13; 

// variable to hold a transmitted byte
char state; 
// default speed variable
int speed = 100;
// BLE Peripheral Device
BLEPeripheral blePeripheral;
// Nordic Semiconductor UART service 
BLEService uartService = BLEService("6E400001-B5A3-F393-E0A9E50E24DCCA9E");
// define characteristics
BLECharacteristic rxCharacteristic = BLECharacteristic("6E400002-B5A3-F393-E0A9E50E24DCCA9E", BLEWrite, 20);  // == TX on central (android app)
BLECharacteristic txCharacteristic = BLECharacteristic("6E400003-B5A3-F393-E0A9E50E24DCCA9E", BLENotify , 20); // == RX on central (android app)
// Motor shield object with the default I2C address
Adafruit_MotorShield MotorShield = Adafruit_MotorShield(); 
// Set 4 motors 
Adafruit_DCMotor *MotorEsquerra  = MotorShield.getMotor(1); // M1 Left front
Adafruit_DCMotor *MotorDret      = MotorShield.getMotor(2); // M2 Right front
Adafruit_DCMotor *MotorEsquerra2 = MotorShield.getMotor(3); // M3 Left rear
Adafruit_DCMotor *MotorDret2     = MotorShield.getMotor(4); // M4 Right rear
// Setup
void setup() {
  Serial.begin(9600);
  Serial.println("Setting BlueTooth BLE!");
  pinMode(ledPin, OUTPUT); // use the LED on pin 13 as an output  
  // set advertised local name and service UUID:
  blePeripheral.setLocalName("BLE_ROV");
  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);
  // assign event handler for characteristic
  rxCharacteristic.setEventHandler(BLEWritten, rxCharacteristicWritten);
  // advertise the service
  blePeripheral.begin();
  Serial.println("Setting Adafruit Motorshield v2 - 2 DC Motor!");
  MotorShield.begin();
  // create with the default frequency 1.6KHz
  //MotorShield.begin(1000);  // OR with a different frequency, say 1KHz  
}

void loop() {
  // poll ble peripheral
  blePeripheral.poll();  
}

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

// Disconnected
void blePeripheralDisconnectHandler(BLECentral& central) {
  // central disconnected event handler
  Serial.print("Disconnected event, central: ");
  Serial.println(central.address());
  //Serial.println("LED off");
  digitalWrite(ledPin, LOW);
}

// List commands from nRF tools uart
// 1 - 25% of motor power HEX:31
// 2 - 50% of motor power HEX:32
// 3 - 75% of motor power HEX:33
// 4 - 100% of motor power HEX:34
// 5 - forward HEX:41
// 6 - left HEX:42
// 7 - stop HEX:43
// 8 - right HEX:44
// 9 - reverse HEX:45

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();  //set state to be the value written from the phone/tablet to the Arduino 101
    Serial.println(char(state));      //print out the character to the serial monitor

    if (state == '1') {
       speed = 65;
    }
    else if (state == '2') {
       speed = 125;
    }
    else if (state == '3') {
       speed = 190;
    }
    else if (state == '4') {
       speed = 255;
    }
    else if (state == '5') {
      Step( "FORWARD", speed, 5000);
    }
    else if (state == '6') {
      Step( "LEFT", speed, 5000);
    }    
    else if (state == '8') {
      Step( "RIGHT", speed, 5000);
    }
    else if (state == '9') {
      Step( "BACKWARD", speed, 5000);
    }
    else if (state == '7') {
      Step( "STOP", speed, 5000);
    }
     Serial.println("Finito.");    
  } 
}

void Step(String direction, int speed, int duration) {
  // Take a step
  // direction: FORWARD, BACKWARD, LEFT, RIGHT
  // speed: 0-255 Motor Speed
  // duration: miliseconds duration of step

  Serial.println("Stepping Direction " + direction);
  Serial.println("Set speed: " + speed);
  MotorEsquerra->setSpeed(speed);
  MotorDret->setSpeed(speed);


  Serial.println("Direcci: " + direction);
  if (direction == "FORWARD"){
    Serial.println("Motor Esquerra FORWARD SPEED: " + speed);
    MotorEsquerra->run(FORWARD);
    Serial.println("Motor Dret FORWARD SPEED: " + speed);
    MotorDret->run(FORWARD);    
  }
  else if (direction == "BACKWARD"){
    MotorEsquerra->run(BACKWARD);
    Serial.println("Motor Esquerra BACKWARD SPEED: " + speed);
    MotorDret->run(BACKWARD);
   Serial.println("Motor Dret BACKWARD SPEED: " + speed);  
  }
  else if (direction == "LEFT"){
    MotorEsquerra->run(BACKWARD);
    MotorDret->run(FORWARD);
  }
  else if (direction == "RIGHT"){
    MotorEsquerra->run(FORWARD);
    MotorDret->run(BACKWARD);
  }
  else if (direction == "STOP"){
   MotorEsquerra->run(RELEASE);
  MotorDret->run(RELEASE);
  }
  else {
    Serial.println("Invalid Step Direction!");
  }    
}