c - Arduino回避机器人随机停止并扫描

Question

您好，我已经构建了一个 arduino 回避机器人，问题是它有时会不断尝试 watchsurrounding(); 虽然他附近甚至没有任何东西，我并不是说它在 watchsurrounding() 时扫描一米；应该只在有东西接近 18 厘米时触发，当 3 米内没有任何东西时，它也会连续扫描。请帮忙，这是我的代码：

#include <Servo.h>
#include <AFMotor.h> //import your motor shield library
AF_DCMotor motor1(1,MOTOR12_8KHZ); // set up motors.
AF_DCMotor motor2(2, MOTOR12_8KHZ);
const int trigger=13;
const int echo=10;

int leftscanval, centerscanval, rightscanval, ldiagonalscanval, rdiagonalscanval;
char choice;
  
const int distancelimit = 18;           
const int sidedistancelimit = 12; 

int distance;
int numcycles = 0;
int stuck = 0;
int stucktesterlast = 0;
char turndirection; 
const int turntime = 800; 
int thereis;
Servo head;

void setup(){
  //Serial.begin(9600);  
  //Serial.println("--- Start Serial Monitor SEND_RCVE ---");
  head.attach(9);
  head.write(80);
  motor1.setSpeed(155); //set the speed of the motors, between 0-255
motor2.setSpeed (155);  
  pinMode(trigger,OUTPUT);
  pinMode(echo,INPUT);
  //Variable inicialization
  digitalWrite(trigger,LOW);
}

void go(){ 
   motor1.run(FORWARD);  
   motor2.run (FORWARD);
}

void backwards(){
  motor1.run(BACKWARD);  
    motor2.run (BACKWARD);
    delay(1200);
    turnleft(turntime);
}

int watch(){
  long howfar;
  long howfarcm;
  digitalWrite(trigger,LOW);
  delayMicroseconds(5);                                                                              
  digitalWrite(trigger,HIGH);
  delayMicroseconds(15);
  digitalWrite(trigger,LOW);
  howfar=pulseIn(echo,HIGH);
  howfarcm=howfar*0.01657; 
  return round(howfarcm);
}

void turnleft(int t){
  motor1.run(FORWARD);  
    motor2.run (BACKWARD);
  delay(t);
}

void turnright(int t){
  motor1.run(BACKWARD); 
    motor2.run (FORWARD);
  delay(t);
}  

void stopmove(){
  motor1.run(RELEASE);  
    motor2.run (RELEASE);
}  

void watchsurrounding(int stuck){  
  int stucktester;
  centerscanval = watch();
  if(centerscanval<distancelimit){stopmove();}
  head.write(120);
  delay(100);
  ldiagonalscanval = watch();
  if(ldiagonalscanval<distancelimit){stopmove();}
  head.write(160);
  delay(300);
  leftscanval = watch();
  if(leftscanval<sidedistancelimit){stopmove();}
  head.write(120);
  delay(100);
  ldiagonalscanval = watch();
  if(ldiagonalscanval<distancelimit){stopmove();}
  head.write(80); 
  delay(100);
  centerscanval = watch();
  if(centerscanval<distancelimit){stopmove();}
  head.write(40);
  delay(100);
  rdiagonalscanval = watch();
  if(rdiagonalscanval<distancelimit){stopmove();}
  head.write(0);
  delay(100);
  rightscanval = watch();
  if(rightscanval<sidedistancelimit){stopmove();}
  //Serial.println(centerscanval);
  //Serial.println(" centerscanval ");
  //Serial.println(leftscanval);
  //Serial.println(" leftscanval ");
  //Serial.println(rightscanval);
  //Serial.println(" rightscanval ");
  //Serial.println(ldiagonalscanval);
  //Serial.println(" ldiagonalscanval ");
  //Serial.println(rdiagonalscanval);
  //Serial.println(" rdiagonalscanval ");
  head.write(80); 
  stucktester = rightscanval+leftscanval+centerscanval+ldiagonalscanval+rdiagonalscanval;
  //Serial.println(stucktester);
  //Serial.println("stucktester");
  //Serial.println(stuck);
  //Serial.println("stuck");
  if (stucktester - stuck > -40 && stucktester - stuck < 40) {
    backwards();
  }
  stucktesterlast = stucktester;
  delay(300);
}

char decide(){
  watchsurrounding(stucktesterlast);
  if(rdiagonalscanval<distancelimit && rightscanval<distancelimit && ldiagonalscanval>distancelimit && leftscanval>distancelimit) {
    choice = 'l';
  }
  else if (ldiagonalscanval<distancelimit && leftscanval<distancelimit && rdiagonalscanval>distancelimit && rightscanval>distancelimit) {
    choice = 'r';
  }
  else if (ldiagonalscanval < 20 || rdiagonalscanval < 20 || centerscanval < 16) {
    choice = 'b';
  }
  else if (leftscanval > centerscanval && leftscanval > rightscanval) {
    choice = 'l';
  }
  else if (rightscanval > centerscanval && rightscanval > leftscanval) {
    choice = 'r';
  }
  else {
    choice = 'f';
  }
  //Serial.println(choice);
  return choice;
}

void loop(){
  go();  
  ++numcycles;
  if(numcycles>250){ 
    watchsurrounding(stucktesterlast);
    numcycles=0; 
  }
  distance = watch(); 
  if (distance<distancelimit){ 
      ++thereis;}
  if (distance>distancelimit){
      thereis=0;} 
  if (thereis > 100){
    stopmove(); 
    turndirection = decide(); 
    switch (turndirection){
      case 'b':
        backwards();
        break;
      case 'l':
        turnleft(turntime);
        break;
      case 'r':
        turnright(turntime);
        break;
      case 'q':
        turnleft(300);
        break;
      case 'e':
        turnright(300);
        break;
      case 'f':
        ; 
        break;
    }
    thereis=0;
  }
}