#include <SoftwareSerial.h>
#include <Wire.h>
#include <NewPing.h>                      //include 超音波標頭檔
#define MAX_DISTANCE 50                   //超音波最遠接收到50cm
NewPing sonar(13, 12, MAX_DISTANCE);      //設定超音波腳位(trig為Pin 13, echo為Pin 12)
int cm;                                   //cm為存取超音波數值的變數
SoftwareSerial BT(0,1);                   ////設定軟體串列通訊給藍芽用腳位設定(trig為Pin 10, echo為Pin 11)
  int strsize;
  int k=15;
  ////////////////////////////////////motor設定//////////////////////////////////////////////////////////
  /*
  直流馬達 的 AIN1 連接至 Arduino 之  的 pin 2
  直流馬達 的 PWM1 連接至 Arduino 之  的 pin 3
  直流馬達 的 BIN1 連接至 Arduino 之  的 pin 4
  直流馬達 的 PWM2 連接至 Arduino 之  的 pin 5
 */
  const int DC_DIR_1 = 7;
const int DC_Motor_1 = 6;
const int DC_DIR_2 = 4;
const int DC_Motor_2 = 5;
 ////////////////////////////////////motor設定//////////////////////////////////////////////////////////
void setup() {
  pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);   // sets the motor pin as output
  BT.begin(9600);
  Serial.begin(9600);
}

void loop() {

  while(true){
    if ((strsize=(BT.available()))>0){
       k=char(BT.read());
       if(k==77)
       {
         cm = sonar.ping_cm();   //接收超音波數值,單位cm
        Serial.print(" urtrasonic detect=  ");
         Serial.print(cm);
          Serial.println(" cm");
          Serial.print(" cmd=  ");
         Serial.println(k);
          BT.write(cm);
          // Serial.print(" test  ");
         //Serial.println(cm);
       }
       else
       {
         Serial.print(" urtrasonic detect=  ");
         Serial.print(cm);
         Serial.println(" cm");
         Serial.print(" cmd=  ");
         Serial.println(k);
       }
      if(k==61)    //a
       {
        digitalWrite(DC_DIR_1,LOW);
       digitalWrite(DC_DIR_2,LOW);
       analogWrite(DC_Motor_1, 150);
       analogWrite(DC_Motor_2, 150);
        delay(50);                                 延遲
       }
       else if(k==62)  //b
       {
        digitalWrite(DC_DIR_1,HIGH);
       digitalWrite(DC_DIR_2,HIGH);
       analogWrite(DC_Motor_1, 150);
       analogWrite(DC_Motor_2, 150);
        delay(50);                                 延遲
       }
       else if(k==63)  //c
       {
        digitalWrite(DC_DIR_1,LOW);
       digitalWrite(DC_DIR_2,HIGH);
       analogWrite(DC_Motor_1, 150);
       analogWrite(DC_Motor_2, 150);
        delay(50);                                 延遲
       }
       else if(k==64)  //d
       {
        digitalWrite(DC_DIR_1,HIGH);
       digitalWrite(DC_DIR_2,LOW);
       analogWrite(DC_Motor_1, 150);               馬達轉速150
       analogWrite(DC_Motor_2, 150);               馬達轉速150
        delay(50);                                 延遲
       }
        else if(k==65)  //e                        感應到65
       {
       analogWrite(DC_Motor_1, 0);                 馬達轉速零
       analogWrite(DC_Motor_2, 0);                 馬達轉速零
        delay(50);                                 延遲
       }


       }


  }