Final Project: Updated Body for the Concept Vehicle

/*********************************************************************
This is an example for our nRF8001 Bluetooth Low Energy Breakout

  Pick one up today in the adafruit shop!
  ------> http://www.adafruit.com/products/1697

Adafruit invests time and resources providing this open source code, 
please support Adafruit and open-source hardware by purchasing 
products from Adafruit!

Written by Kevin Townsend/KTOWN  for Adafruit Industries.
MIT license, check LICENSE for more information
All text above, and the splash screen below must be included in any redistribution
*********************************************************************/

// This version uses call-backs on the event and RX so there's no data handling in the main loop!

#include <SPI.h>
#include "Adafruit_BLE_UART.h"
#include <Servo.h> 
#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
  #include <avr/power.h>
#endif

// Which pin on the Arduino is connected to the NeoPixels?
// On a Trinket or Gemma we suggest changing this to 1
#define PIN            1

// How many NeoPixels are attached to the Arduino?
#define NUMPIXELS      16

Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
int delayval = 500; // delay for half a second

Servo myservo;
int pos = 135;   

#define ADAFRUITBLE_REQ 10
#define ADAFRUITBLE_RDY 2
#define ADAFRUITBLE_RST 9

Adafruit_BLE_UART uart = Adafruit_BLE_UART(ADAFRUITBLE_REQ, ADAFRUITBLE_RDY, ADAFRUITBLE_RST);

// connect motor controller pins to Arduino digital pins
// motor one
int enA = 5;
int in1 = 4;
int in2 = 7;
// motor two
int enB = 6;
int in3 = 3;
int in4 = 8;

boolean messageReceived = false;
uint8_t message[20];

/**************************************************************************/
/*!
    This function is called whenever select ACI events happen
*/
/**************************************************************************/
void aciCallback(aci_evt_opcode_t event)
{
  switch(event)
  {
    case ACI_EVT_DEVICE_STARTED:
      Serial.println(F("Advertising started"));
      break;
    case ACI_EVT_CONNECTED:
      Serial.println(F("Connected!"));
      break;
    case ACI_EVT_DISCONNECTED:
      Serial.println(F("Disconnected or advertising timed out"));
      break;
    default:
      break;
  }
}

void changeLights() {
  int pixels1[16];
  if (pos <= 140 && pos >= 131) {
    pixels1[0] = 0;
  }
  if (pos <= 150 && pos >= 141) {
    pixels1[0] = 0;
    pixels1[1] = 1;
  }
   if (pos <= 160 && pos >= 151) {
    pixels1[0] = 0;
    pixels1[1] = 1;
    pixels1[2] = 2;
  }
   if (pos <= 170 && pos >= 161) {
    pixels1[0] = 0;
    pixels1[1] = 1;
    pixels1[2] = 2;
    pixels1[3] = 3;
  }
  if (pos >= 171) {
    pixels1[0] = 0;
    pixels1[1] = 1;
    pixels1[2] = 2;
    pixels1[3] = 3;
    pixels1[4] = 4;
  }
  if (pos <= 130 && pos >= 121) {
    pixels1[0] = 0;
    pixels1[1] = 15;
  }
  if (pos <= 120 && pos >= 111) {
    pixels1[0] = 0;
    pixels1[1] = 15;
    pixels1[2] = 14;
  }
  if (pos <= 110 && pos >= 101) {
    pixels1[0] = 0;
    pixels1[1] = 15;
    pixels1[2] = 14;
    pixels1[3] = 13;
  }
  if (pos <= 100) {
    pixels1[0] = 0;
    pixels1[1] = 15;
    pixels1[2] = 14;
    pixels1[3] = 13;
    pixels1[4] = 12;
  }
  for (int i=0; i < sizeof(pixels1); i++) {
        pixels.setPixelColor(pixels1[i], pixels.Color(195,144,212)); // Moderately bright purple color.

    pixels.show(); // This sends the updated pixel color to the hardware.

    delay(5); // Delay for a period of time (in milliseconds).
  }
}

void forward() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}

void backward() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);  
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH); 
}

void motorsOn() {
  analogWrite(enB, 200);
  analogWrite(enA, 200);
}

void motorsOff() {
  digitalWrite(enA, 0);
  digitalWrite(enB, 0);
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);  
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}
/**************************************************************************/
/*!
    This function is called whenever data arrives on the RX channel
*/
/**************************************************************************/
void rxCallback(uint8_t *buffer, uint8_t len)
{
  Serial.print(F("Received "));
  Serial.print(len);
  Serial.println(F(" bytes: "));
//  for(int i=0; i<len; i++)
//   Serial.print((char)buffer[i]); 
//
//  Serial.print(F(" ["));
//
//  for(int i=0; i<len; i++)
//  {
//    Serial.print(" 0x"); Serial.print((char)buffer[i], HEX); 
//  }
//  Serial.println(F(" ]"));

    switch(buffer[2]) {
      case 53:
        Serial.println ( "up");
        backward();
        motorsOn();
        break;
      case 54:
        Serial.println ( "down");
        forward();
        motorsOn();
        break;
      case 55:
        Serial.println ( "left");
        if (pos >= 90) {
          pos -= 10;
        }
        myservo.write(pos);
//        changeLights();
        break;
      case 56:
        Serial.println ( "right");
        if (pos <= 170) {
          pos += 10;
        }
        myservo.write(pos);
//        changeLights();
        break;
      default:
        motorsOff();
        break;
    }
  
  /* Echo the same data back! */
  uart.write(buffer, len);
}

/**************************************************************************/
/*!
    Configure the Arduino and start advertising with the radio
*/
/**************************************************************************/
void setup(void)
{ 
  Serial.begin(9600);
  while(!Serial); // Leonardo/Micro should wait for serial init
  Serial.println(F("Adafruit Bluefruit Low Energy nRF8001 Callback Echo demo"));

  uart.setRXcallback(rxCallback);
  uart.setACIcallback(aciCallback);
  // set all the motor control pins to outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  myservo.attach(A0);  // attaches the servo on pin A0 to the servo object 
  delay(2000);
  uart.setDeviceName("wsup"); /* 7 characters max! */
  uart.begin();
  pixels.begin();
}

/**************************************************************************/
/*!
    Constantly checks for new events on the nRF8001
*/
/**************************************************************************/


void loop()
{
  uart.pollACI();
 
}