// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2020-06-30 오후 7:56:14
// by Ok-Hyun Park
//
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
SoftwareSerial btSerial(A4,A5);
//
int mq3Pin=A3; // MQ-3 센서핀을 아두이노 보드의 A3 핀으로 설정
int redPin=7; // LED 센서의 R 핀을 아두이노 보드의 7번 핀으로 설정
int greenPin=6; // LED 센서의 G 핀을 아두이노 보드의 6번 핀으로 설정
int bluePin=5; // LED 센서의 B 핀을 아두이노 보드의 5번 핀으로 설정
//
String keyPressed;
//
String btGet(){
  String CMD;
  if(btSerial.available()>0){
    char command=btSerial.read();
    switch(command){
      case 'R': CMD="Forward"; break;
      case 'Q': CMD="Backward"; break;
      case 'P': CMD="Right"; break;
      case 'O': CMD="Left"; break;
      case 'S': CMD="Stop"; break;
      default: CMD="Stop"; break;
    }
  }
  return CMD;
}
//
void move(int direction,int speed){
  if     (direction==1){ analogWrite(6,speed); analogWrite(9,0); analogWrite(10,speed); analogWrite(11,0); }
  else if(direction==2){ analogWrite(6,0); analogWrite(9,speed); analogWrite(10,0); analogWrite(11,speed); }
  else if(direction==3){ analogWrite(6,speed); analogWrite(9,0); analogWrite(10,0); analogWrite(11,0); }
  else if(direction==4){ analogWrite(6,0); analogWrite(9,0); analogWrite(10,speed); analogWrite(11,0); }
  else if(direction==5){ analogWrite(6,speed); analogWrite(9,0); analogWrite(10,0); analogWrite(11,speed); }
  else if(direction==6){ analogWrite(6,0); analogWrite(9,speed); analogWrite(10,speed); analogWrite(11,0); }
}
//
long Ranging(int Trig_pin,int Echo_pin){
  long duration,distacne_cm;
  long Time_out;
  Time_out=30000;
  pinMode(Trig_pin,OUTPUT); pinMode(Echo_pin,INPUT);
  digitalWrite(Trig_pin,LOW); delayMicroseconds(2);
  digitalWrite(Trig_pin,HIGH); delayMicroseconds(10); digitalWrite(Trig_pin,LOW);
  duration=pulseIn(Echo_pin,HIGH,Time_out);
  if(duration==0){ duration=Time_out; }
  distacne_cm=duration /29/2;
  return distacne_cm;
}
//
void _delay(float seconds){
  long endTime=millis()+seconds*1000;
  while(millis()<endTime)_loop();
}
//
void setup(){
  btSerial.begin(9600);
  pinMode(6,OUTPUT); pinMode(9,OUTPUT); pinMode(10,OUTPUT); pinMode(11,OUTPUT);
  analogWrite(6,LOW); analogWrite(9,LOW); analogWrite(10,LOW); analogWrite(11,LOW);
  pinMode(A0,OUTPUT); pinMode(A1,OUTPUT); pinMode(A2,OUTPUT);
  keyPressed=btGet();
  btSerial.println("AT"); _delay(2);
  String btcmd="AT+NAME";
  btcmd.concat("SPAC01");
  btSerial.println(btcmd); _delay(2);
}
//
void loop(){
  int v=analogRead(mq3Pin);
  if(v>400){
    analogWrite(6,0); analogWrite(9,0); analogWrite(10,0); analogWrite(11,0); // STOP
  }
  else{
    keyPressed=btGet();
    if(keyPressed=="Forward"){ move(1,100/100.0*255); }
    if(keyPressed=="Left"){ move(3,100/100.0*255); }
    if(keyPressed=="Right"){ move(4,100/100.0*255); }
    if(Ranging(3,4)<10){ move(1,0/100.0*255); }
    if(Ranging(3,4)<60){ analogWrite(A0,0); analogWrite(A1,0); analogWrite(A2,255); }
    if(Ranging(3,4)<40){ analogWrite(A0,0); analogWrite(A1,255); analogWrite(A2,0); }
    if(Ranging(3,4)<20){ analogWrite(A0,255); analogWrite(A1,0); analogWrite(A2,0); }
    if(keyPressed=="Backward"){ move(2,100/100.0*255); }
  }
}