// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2018-10-22 오전 10:44:52
// by Ok-Hyun Park
//
#include <SoftwareSerial.h>              // 시리얼 통신을 위한 라이브러리 선언
#define BT_RXD  3                        // 아두이노의 4번핀을 RX(받는 핀)로 설정
#define BT_TXD  4                        // 아두이노 3번핀을 TX(보내는 핀)로 설정
SoftwareSerial bluetooth(BT_RXD,BT_TXD); // 블루투스 통신을 위한 설정
//
#define CAR_DIR_FW   1 // 전진.
#define CAR_DIR_BW   2 // 후진.
#define CAR_DIR_LF   3 // 좌회전.
#define CAR_DIR_RF   4 // 우회전
#define CAR_DIR_ST   5 // 정지.
#define CAR_DIR_BWRT 6 // 후진+우회전
//
char E_carDir=0;
int  Edu_count=0;
//
int RmEN=5,Rm1_pin= 8,Rm2_pin= 9; // 오른쪽 모터 제어선 IN2
int LmEN=6,Lm3_pin=10,Lm4_pin=11; // 왼쪽 모터의 Enable&PWM
int Gled=12,Yled=7,Rled=13;
int sensorpin=2;
int Speed=153;               // 최대 속도의  60%
int pSpeed=0;
//
void SmartCar_Stop(){ // 정지
  if(E_carDir==CAR_DIR_FW||E_carDir==CAR_DIR_LF||E_carDir==CAR_DIR_RF){
    for(int i=Speed; i>=0; i=i-5){ analogWrite(RmEN,i); analogWrite(LmEN,i); delay(20); }
  }
  else if(E_carDir==CAR_DIR_BW){
    for(int i=Speed; i>=0; i=i-5){ analogWrite(RmEN,i); analogWrite(LmEN,i); delay(20); }
  }
  digitalWrite(Gled,LOW); digitalWrite(Rled,HIGH); digitalWrite(Yled,LOW);
  digitalWrite(RmEN,LOW); digitalWrite(LmEN,LOW); // 정지
}
//
void SmartCar_Go(){ // 전진
  digitalWrite(Rm1_pin,HIGH); digitalWrite(Rm2_pin,LOW);
  digitalWrite(Lm3_pin,HIGH); digitalWrite(Lm4_pin,LOW);
  for(int i=pSpeed; i<=Speed; i=i+5){ analogWrite(RmEN,i); analogWrite(LmEN,i); delay(20); }
  digitalWrite(Gled,HIGH); digitalWrite(Rled,LOW); digitalWrite(Yled,LOW);
  pSpeed=Speed;
}
//
void SmartCar_Back(){ // 후진
  digitalWrite(Rm1_pin,LOW); digitalWrite(Rm2_pin,HIGH);
  digitalWrite(Lm3_pin,LOW); digitalWrite(Lm4_pin,HIGH);
  for(int i=pSpeed;i<=Speed;i=i+5){ analogWrite(RmEN,i); analogWrite(LmEN,i); delay(20); }
  digitalWrite(Gled,HIGH); digitalWrite(Rled,LOW); digitalWrite(Yled,HIGH);
  pSpeed=Speed;
}
//
void SmartCar_Left(){ // 좌회전
  digitalWrite(Rm1_pin,HIGH); digitalWrite(Rm2_pin,LOW);
  digitalWrite(Lm3_pin,HIGH); digitalWrite(Lm4_pin,LOW);
  for(int i=pSpeed;i<=Speed;i=i+5){ analogWrite(RmEN,i*1.4); analogWrite(LmEN,i*0.2); delay(50); } // 140%// 20%
  digitalWrite(Gled,LOW); digitalWrite(Rled,LOW); digitalWrite(Yled,HIGH);
  pSpeed=Speed;
}
//
void SmartCar_Right(){ // 우회전
  digitalWrite(Rm1_pin,HIGH); digitalWrite(Rm2_pin,LOW);
  digitalWrite(Lm3_pin,HIGH); digitalWrite(Lm4_pin,LOW);
  for(int i=pSpeed;i<=Speed;i=i+5){ analogWrite(RmEN,i*0.2); analogWrite(LmEN,i*1.4); delay(50); } // 20%// 140%
  digitalWrite(Gled,LOW); digitalWrite(Rled,LOW); digitalWrite(Yled,HIGH);
  pSpeed=Speed;
}
// 제어명령을 실제로 수행하는 함수
void controllerByBTCommand(char szBTCmd){
  if(szBTCmd=='+'){ // 모터의 speed up
    pSpeed=Speed; Speed+=20; Speed=min(Speed,255); Serial.print("Speed Up "); Serial.println(Speed);
  }
  else if(szBTCmd=='-'){ // 모터의 speed down
    pSpeed=Speed; Speed-=20; Speed=max(Speed,50); Serial.print("Speed down "); Serial.println(Speed);
  }
  else if(szBTCmd=='g')E_carDir=CAR_DIR_FW; // 전진
  else if(szBTCmd=='s')E_carDir=CAR_DIR_ST; // 정지
  else if(szBTCmd=='b')E_carDir=CAR_DIR_BW; // 후진
  else if(szBTCmd=='l')E_carDir=CAR_DIR_LF; // 좌회전
  else if(szBTCmd=='r')E_carDir=CAR_DIR_RF; // 우회전
}
// 방향 전환에 따른 동작 명령 함수
//
void Sensor(){
  int state=digitalRead(sensorpin); Serial.print("sensorpin="); Serial.println(state);
  if(state==0){ E_carDir=CAR_DIR_BWRT; }
  Serial.println("Warning");
  pSpeed=Speed;
}
//
void SmartCar_Update(){
  Sensor();
  if     (E_carDir==CAR_DIR_FW)SmartCar_Go();    // 전진
  else if(E_carDir==CAR_DIR_BW)SmartCar_Back();  // 후진
  else if(E_carDir==CAR_DIR_LF)SmartCar_Left();  // 좌회전
  else if(E_carDir==CAR_DIR_RF)SmartCar_Right(); // 우회전
  else if(E_carDir==CAR_DIR_ST)SmartCar_Stop();  // 정지.
  else if(E_carDir==CAR_DIR_BWRT){
    SmartCar_Back();  // 후진
    delay(1000);
    SmartCar_Right(); // 우회전
  }
}
//
void setup(){
  // declare the ledPin as an OUTPUT:
  pinMode(RmEN,OUTPUT); pinMode(Rm1_pin,OUTPUT); pinMode(Rm2_pin,OUTPUT);
  pinMode(LmEN,OUTPUT); pinMode(Lm3_pin,OUTPUT); pinMode(Lm4_pin,OUTPUT);
  pinMode(Gled,OUTPUT); pinMode(Yled,OUTPUT);    pinMode(Rled,OUTPUT);
  pinMode(sensorpin,INPUT);
  Serial.begin(9600);       // PC와 아두이노간 시리얼 통신속도를 9600bps로 설정
  bluetooth.begin(9600);    // 블루투스와 아두이노간 시리얼 통신속도를 9600bps로 설정
  digitalWrite(RmEN,HIGH);  // 오른쪽 모터의 Enable 핀 활성화
  digitalWrite(LmEN,HIGH);  // 왼쪽 모터의 Enable 핀 활성화
}
//
void loop(){
  char szRecvCmd=bluetooth.read(); // 블루투스 통신을 통한 명령 받기
  controllerByBTCommand(szRecvCmd);
  SmartCar_Update();               // 스마트카 상태 업데이트
  if     ((Edu_count%20)==0){ Edu_count=0; delay(10); }
  else if((Edu_count%10)==0){ delay(10); }
  Edu_count++;
}