//超音波小車（ARDUINO）
//    L = 左　R = 右　F = 前　B = 後

#include <Servo.h> 
int pinLB=5;     // 定義5腳位 左後
int pinLF=6;     // 定義6腳位 左前

int pinRB=10;    // 定義10腳位 右後
int pinRF=11;    // 定義11腳位 右前

int inputPin = 13;  // 定義超音波信號接收腳位 Rx
int outputPin =12;  // 定義超音波信號發射腳位 Tx

int Fspeedd = 0;      // 前速
int Rspeedd = 0;      // 右速
int Lspeedd = 0;      // 左速
int directionn = 0;   // 前=8 後=2 左=4 右=6 
Servo myservo;        // 設 myservo
int delay_time = 250; // 伺服馬達轉向後的穩定時間

int Fgo = 2;         // 前進
int Rgo = 6;         // 右轉
int Lgo = 4;         // 左轉
int Bgo = 8;         // 倒車

void setup()
 {
  Serial.begin(9600);     // 定義馬達輸出腳位 
  pinMode(pinLB,OUTPUT); // 腳位 5 (PWM)
  pinMode(pinLF,OUTPUT); // 腳位 6 (PWM)
  pinMode(pinRB,OUTPUT); // 腳位 10 (PWM) 
  pinMode(pinRF,OUTPUT); // 腳位 11 (PWM)
  
  pinMode(inputPin, INPUT);    // 定義超音波輸入腳位
  pinMode(outputPin, OUTPUT);  // 定義超音波輸出腳位   

  myservo.attach(9);    // 定義伺服馬達輸出第9腳位(PWM)
 }





    

    
void detection()        //測量3個角度(0.90.179)
    {      
      int delay_time = 250;   // 伺服馬達轉向後的穩定時間
      ask_pin_F();            // 讀取前方距離

    }    
void ask_pin_F()   // 量出前方距離 
    {
      myservo.write(120);
      digitalWrite(outputPin, LOW);   // 讓超聲波發射低電壓2μs
      delayMicroseconds(2);
      digitalWrite(outputPin, HIGH);  // 讓超聲波發射高電壓10μs，這裡至少是10μs
      delayMicroseconds(10);
      digitalWrite(outputPin, LOW);    // 維持超聲波發射低電壓
      float Fdistance = pulseIn(inputPin, HIGH);  // 讀差相差時間
      Fdistance= Fdistance/5.8/10;       // 將時間轉為距離距离（單位：公分）
      Serial.print("Fx distance:");      //輸出距離（單位：公分）
      Serial.println(Fdistance);         //顯示距離
      //Fspeedd = Fdistance;              // 將距離 讀入Fspeedd(前速)
    }  
 
 
    
void loop()
 {
    myservo.write(50);  //讓伺服馬達回歸 預備位置 準備下一次的測量
    detection();        //測量角度 並且判斷要往哪一方向移動
      
 

 

 }