// RC receiver over Bluetooth for RoboRemo
// www.roboremo.app

// this receiver has 4 channels,
// but you can easily add more.

// Hardware setup:
// BT module   Arduino
// GND ------- GND
// VCC ------- 5V
// TX-O ------ pin0
// RX-I ------ pin1

// RC ch out     Arduino
// ch0 --------- pin4
// ch1 --------- pin5
// ch2 --------- pin6
// ch3 --------- pin7


#define bluetooth Serial
#include <Servo.h>

const int chCount = 4;

Servo ch[chCount];
int chPin[chCount] = {4, 5, 6, 7};
int chVal[chCount];

// config:
int usMin = 1000;
int usMax = 2000;



char cmd[100];
int cmdIndex;
long lastCmdTime = 60000;


boolean cmdStartsWith(char *st) {
  for(int i=0; ; i++) {
    if(st[i]==0) return true;
    if(cmd[i]==0) return false;
    if(cmd[i]!=st[i]) return false;;
  }
  return false;
}




void exeCmd() { 
  
  lastCmdTime = millis();
  
  // example: set slider id to "ch0", set min 1000 and set max 2000
  
  if( cmdStartsWith("ch") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = (int)atof(cmd+4);
    }
  }
  
  // invert channel:
  // example: set slider id to "ci0", set min -2000 and set max -1000
  
  if( cmdStartsWith("ci") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = -(int)atof(cmd+4);
    }
  }
  
  // use accelerometer:
  // example: set acc y id to "ca1"
  
  if( cmdStartsWith("ca") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = (usMax+usMin)/2 + (int)( atof(cmd+4)*51 ); // 9.8*51 = 500 => 1000 .. 2000
    }
  }
  
  // invert accelerometer:
  // example: set acc y id to "cb1"
  
  if( cmdStartsWith("cb") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = (usMax+usMin)/2 - (int)( atof(cmd+4)*51 ); // 9.8*51 = 500 => 1000 .. 2000
    }
  }
  
  // update:
  // send something that starts with "update" periodically,
  // in order to update channels
  // example: set acc x id to "update"
  
  if( cmdStartsWith("update") )
    for(int i=0; i<chCount; i++) 
      ch[i].writeMicroseconds(chVal[i]);
}



void setup() {
  
  delay(500); // wait for bluetooth module to start

  // BlueSMiRF default baud is 115200
  // For HC-05, please try 9600

  bluetooth.begin(115200); 
  
  for(int i=0; i<chCount; i++) {
    // attach channels to pins
    ch[i].attach(chPin[i], usMin, usMax);
    // initial value = middle
    chVal[i] = (usMin + usMax)/2;
    // update
    ch[i].writeMicroseconds( chVal[i] );
  }
  
  cmdIndex = 0;
}


char c = 0;


void loop() {
  
  // if contact lost for more than half second
  if(millis() - lastCmdTime > 500) {  
    for(int i=0; i<chCount; i++) {
      // set all values to middle
      ch[i].writeMicroseconds( (usMin + usMax)/2 );
    }
  }
  
  
  
  if(bluetooth.available()) {
    
    c = (char)bluetooth.read();
    
    if(c=='\n') {
      cmd[cmdIndex] = 0;
      exeCmd();  // execute the command
      cmdIndex = 0; // reset the cmdIndex
    } else {      
      cmd[cmdIndex] = c;
      if(cmdIndex<99) cmdIndex++;
    }
    
  }
  
}