int t= 3;//t會影響轉速，t越小轉越快，太小會轉不動

int gate_P = A0;

int button_P2 = 2;

int button_P = 3;

int button;

int button2;

int gate;

int trig = 7; // trig pin for HC-SR04

int echo = 6; // echo pin for HC-SR04

float duration,distance;

void setup() {

  pinMode(button_P,INPUT_PULLUP);

  pinMode(button_P2,INPUT_PULLUP);

  pinMode(gate_P,INPUT);

  pinMode(8,OUTPUT);

  pinMode(9,OUTPUT);

  pinMode(10,OUTPUT);

  pinMode(11,OUTPUT);

  pinMode(trig,OUTPUT);

  pinMode(echo,INPUT);

  Serial.begin(115200);

}

void loop() {

  button = digitalRead(button_P);

  button2 = digitalRead(button_P2);

  gate = analogRead(gate_P);

  float pos =0;

  if(button == LOW){

    gate = 0;

    while(gate <500){

      stepForward();

      gate = analogRead(gate_P);

      //Serial.println(gate);

    }

    delay(1000);

    for(int i = 0;i <= 128;i ++){

      stepForward();

    }

    //Serial.println(button2);

  delay(50);

  }

  if(button2 == LOW){

    Serial.println("Radar Start");

    delay(50);

    for(int i=0;i<=256;i++){

      stepBack();

      pos = float(i)/256*180;

      dist_calc(pos);

      }

    }

}

float dist_calc(float pos){

  // trigger 40kHz pulse for ranging

  digitalWrite(trig,LOW);

  delayMicroseconds(2);

  digitalWrite(trig,HIGH);

  delayMicroseconds(10);

  digitalWrite(trig,LOW);

  // convert from duration for pulse to reach detector (microseconds) to range (in cm)

  duration = pulseIn(echo,HIGH,500000); // duration for pulse to reach detector (in microseconds)

  distance = 100.0*(343.0*(duration/2.0))/1000000.0; // 100.0*(speed of sound*duration/2)/microsec conversion

  delay(50);

  Serial.print(pos); // position of servo motor

  Serial.print(","); // comma separate variables

  Serial.println(distance); // print distance in cm

}

void stepForward(){

  digitalWrite(8,1);

  digitalWrite(9,0);

  digitalWrite(10,0);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,1);

  digitalWrite(9,1);

  digitalWrite(10,0);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,1);

  digitalWrite(10,0);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,1);

  digitalWrite(10,1);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,0);

  digitalWrite(10,1);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,0);

  digitalWrite(10,1);

  digitalWrite(11,1);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,0);

  digitalWrite(10,0);

  digitalWrite(11,1);

  delay(t);

  digitalWrite(8,1);

  digitalWrite(9,0);

  digitalWrite(10,0);

  digitalWrite(11,1);

  delay(t); 

}

void stepBack(){

  digitalWrite(8,1);

  digitalWrite(9,0);

  digitalWrite(10,0);

  digitalWrite(11,1);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,0);

  digitalWrite(10,0);

  digitalWrite(11,1);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,0);

  digitalWrite(10,1);

  digitalWrite(11,1);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,0);

  digitalWrite(10,1);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,1);

  digitalWrite(10,1);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,0);

  digitalWrite(9,1);

  digitalWrite(10,0);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,1);

  digitalWrite(9,1);

  digitalWrite(10,0);

  digitalWrite(11,0);

  delay(t);

  digitalWrite(8,1);

  digitalWrite(9,0);

  digitalWrite(10,0);

  digitalWrite(11,0);

  delay(t);

}