Homework 9: Final Vehicle V.2

#include <SPI.h>
#include "Adafruit_BLE_UART.h"

#define ADAFRUITBLE_REQ 10
#define ADAFRUITBLE_RDY 2
#define ADAFRUITBLE_RST 8
#define LEFT_IN 3
#define LEFT_OUT 5
#define RIGHT_OUT 6
#define RIGHT_IN 9

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

void setup(void) { 
  Serial.begin(9600);
  while(!Serial);
  Serial.println(F("EricCar Console"));

  BTLEserial.setDeviceName("EricCar"); /* 7 characters max! */

  BTLEserial.begin();
}

aci_evt_opcode_t laststatus = ACI_EVT_DISCONNECTED;
boolean forward = 0;
boolean backward = 0;
boolean right = 0;
boolean left = 0;

void loop() {
  // Tell the nRF8001 to do whatever it should be working on.
  BTLEserial.pollACI();

  // Ask what is our current status
  aci_evt_opcode_t status = BTLEserial.getState();
  if (status != laststatus) {
    if (status == ACI_EVT_DEVICE_STARTED) {
        Serial.println(F("* Advertising started"));
    }
    if (status == ACI_EVT_CONNECTED) {
        Serial.println(F("* Connected!"));
    }
    if (status == ACI_EVT_DISCONNECTED) {
        Serial.println(F("* Disconnected or advertising timed out"));
    }
    laststatus = status;
  }

  if (status == ACI_EVT_CONNECTED) {
    if (BTLEserial.available()) {      
      String command = "";
      
      while (BTLEserial.available()) {
        command += BTLEserial.read();
      }
      Serial.println(command);
      if (command.equals("3366534854")) {
        // up pressed
      }
      else if (command.equals("3366534855")) {
        // up lifted
      }
      else if (command.equals("3366544953")) {
        // down pressed
      }
      else if (command.equals("3366544854")) {
        // down lifted
      }
      else if (command.equals("3366564951")) {
        // right pressed
        right = 1;
      }
      else if (command.equals("3366564852")) {
        // right lifted
        right = 0;
      }
      else if (command.equals("3366554952")) {
        // left pressed
        left = 1;
      }
      else if (command.equals("3366554853")) {
        // left lifted
        left = 0;
      }
      else if (command.equals("3366494958")) {
        // 1 pressed
        backward = 1;
      }
      else if (command.equals("3366494859")) {
        // 1 lifted
        backward = 0;
      }
      else if (command.equals("3366504957")) {
        // 2 pressed
        forward = 1;
      }
      else if (command.equals("3366504858")) {
        // 2 lifted
        forward = 0;
      }
      else if (command.equals("3366514956")) {
        // 3 pressed
      }
      else if (command.equals("3366514857")) {
        // 3 lifted
      }
      else if (command.equals("3366524955")) {
        // 4 pressed
      }
      else if (command.equals("3366524856")) {
        // 4 lifted
      }
      int left_val = 255;
      int right_val = 255;
      if (left == 1) {
        left_val = int(left_val / 2);
      } else if (right == 1) {
        right_val = int(right_val / 2);
      }
      if (forward == 1) {
        move_wheel(LEFT_IN, LEFT_OUT, left_val, true, true);
        move_wheel(RIGHT_IN, RIGHT_OUT, right_val, true, false);
      } else if (backward == 1) {
        move_wheel(LEFT_IN, LEFT_OUT, left_val, false, true);
        move_wheel(RIGHT_IN, RIGHT_OUT, right_val, false, false);
      } else {
        move_wheel(LEFT_IN, LEFT_OUT, 0, true, true);
        move_wheel(RIGHT_IN, RIGHT_OUT, 0, true, false);
      }
    }
  }
}

void move_wheel(int in, int out, int val, boolean forward, boolean left) {
  Serial.print((left) ? "left" : "right");
  Serial.print(": ");
  Serial.print((forward) ? val : -val);
  Serial.print("\n");
  
  analogWrite(in, (forward) ? val : 0);
  analogWrite(out, (forward) ? 0 : val);
  
  return;
}