自走車超音波避障

#include <AFMotor.h>


#define MAXDISTANCE 200
long duration, cm, value = 10;
int Value;

AF_DCMotor motorLeft(2);
AF_DCMotor motorRight(3);

int measureDistance(){
  digitalWrite(A1, LOW);
  delayMicroseconds(5);
  digitalWrite(A1, HIGH);
  delayMicroseconds(10);
  digitalWrite(A1, LOW);

  pinMode(A0, INPUT); 
  duration = pulseIn(A0, HIGH);
  cm = (duration/2) / 29.1;
}

void setup() {
  Serial.begin (9600);             
  pinMode(A1, OUTPUT);        
  pinMode(A0, INPUT);
  motorLeft.setSpeed(150);
  //motorLeft.run(RELEASE);
  motorLeft.run(FORWARD);
  motorRight.setSpeed(200);
  //motorRight.run(RELEASE);
  motorRight.run(FORWARD);
}

void turnLeft(){
  motorLeft.setSpeed(75);
  motorRight.setSpeed(125);
  motorLeft.run(FORWARD);
  motorRight.run(BACKWARD);
  delay(100);
  motorLeft.run(FORWARD);
  motorRight.run(FORWARD);
  
}

#define ALPHA 0.7

void loop() {
  delay(500);
  measureDistance(); 
  value = (int)(cm*ALPHA + value*(1-ALPHA));
  //value = 10;
  //motorLeft.setSpeed(map(value,0,100,0,150));
  //motorRight.setSpeed(map(value,0,100,0,250));
  motorLeft.setSpeed(map(value,0,MAXDISTANCE,0,150));
  motorRight.setSpeed(map(value,0,MAXDISTANCE,0,250));

  while(cm < 35){
    turnLeft();
    measureDistance();
    Serial.print(cm);
    Serial.println("cm");
  }

  Serial.print(" Value: ");
  Serial.print(value);
  Serial.print(" ");
  Serial.print(cm);
  Serial.println("cm");
  

}