#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);                                 延遲
      }


    }


  }