反馈型智能跟随书包创意设计 简单

int readRange(int Trig,int Echo)
{
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
int distance = pulseIn(Echo, HIGH);
return distance/58;
}

volatile int i;
volatile int x;
volatile int y;
int speed_6;
int speed_11;

void setup() {
  i = 0;
  x = 30;
  y = 15;
  pinMode(A4, INPUT);
  pinMode(A5, OUTPUT);
  pinMode(6, OUTPUT);
pinMode(5, OUTPUT);
digitalWrite(6, 0);
digitalWrite(5, 0);

  pinMode(11, OUTPUT);
pinMode(3, OUTPUT);
digitalWrite(11, 0);
digitalWrite(3, 0);
}

void loop() {
  while (true) {
    i = readRange(A5, A4);
    if (i > x - y && i < x + y) {
      if (i < x) {
        speed_6=255;
        digitalWrite(5, 0);
        analogWrite(6, speed_6);
        speed_11=255;
        digitalWrite(3, 0);
        analogWrite(11, speed_11);
      } else if (i > x) {
        speed_6=255;
        digitalWrite(6, 0);
        analogWrite(5, speed_6);
        speed_11=255;
        digitalWrite(11, 0);
        analogWrite(3, speed_11);
      }
    } else {
      digitalWrite(6, 0);
      digitalWrite(5, 0);
      digitalWrite(11, 0);
      digitalWrite(3, 0);
    }
  }

}