// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2018-06-20 오후 10:05:25
// by Ok-Hyun Park
//
#include <SPI.h>
#include "RF24.h"
#include <SoftwareSerial.h>
SoftwareSerial btSerial(2,10);
const int IN1=9; //Right Motor(-)
const int IN2=6; //Right Motor(+)
const int IN3=5; //Left Motor(+)
const int IN4=3; //Left Motor(-)
int msg[4];
char data=0;
unsigned long pre_time=0,cur_time;
long interval=5000;
RF24 radio(7,8);
const uint64_t pipe=0xE8E8F0F0E1LL;
//
void setup(){
  pinMode(IN1,OUTPUT); pinMode(IN2,OUTPUT); pinMode(IN3,OUTPUT); pinMode(IN4,OUTPUT);
  Serial.begin(19200);
  btSerial.begin(19200);
  radio.begin();
  radio.openReadingPipe(1,pipe);
  radio.startListening();
}
//
void loop(){
  cur_time=millis();
  char c=Serial.read();
  if(radio.available()){
    radio.read(msg,sizeof(msg));
    if(btSerial.available()){
      data=btSerial.read();
    }
  }
  //
  if(data=='A'){
    if(msg[0]>0&&msg[1]>0&&msg[2]>0&&msg[3]>0){ //forward  다 피면 앞으로
      analogWrite(IN1,150); analogWrite(IN2,0); analogWrite(IN3,150); analogWrite(IN4,0);
      pre_time=cur_time; Serial.print("Go"); data=0;
    }
  }
  if(data=='B'){
    if(msg[0]>0&&msg[1]==0&&msg[2]==0&&msg[3]<7){ //backward 엄지 피면 뒤로
      analogWrite(IN1,0); analogWrite(IN2,150); analogWrite(IN3,0); analogWrite(IN4,150);
      pre_time=cur_time; Serial.print("Back"); data=0;
    }
  }
  if(data=='C'){
    if(msg[0]>0&&msg[1]>0&&msg[2]>0&&msg[3]<7){ //left_엄지 검지 중지 피면 왼쪽
      analogWrite(IN1,0); analogWrite(IN2,150); analogWrite(IN3,150); analogWrite(IN4,0);
      pre_time=cur_time; Serial.print("Left"); data=0;
    }
  }
  if(data=='D'){
    if(msg[0]==0&&msg[1]==0&&msg[2]>0&&msg[3]>0){ //right_중지 약지 피면 오른쪽
      analogWrite(IN1,150); analogWrite(IN2,0); analogWrite(IN3,0); analogWrite(IN4,150);
      pre_time=cur_time; Serial.print("Right"); data=0;
    }
  }
  //
  if(pre_time>0&&(cur_time-pre_time>interval)){
    pre_time=cur_time;
    analogWrite(IN1,0); analogWrite(IN2,0); analogWrite(IN3,0); analogWrite(IN4,0);
  }
  //
  //if(msg[0]==0&&msg[1]==0&&msg[2]==0&&msg[3]<7){ //stop_다 접으면 정지
  //  analogWrite(IN1,0); analogWrite(IN2,0); analogWrite(IN3,0); analogWrite(IN4,0);
  //  Serial.print("Stop");
  //}
}