使用arduino Mega的矩形图案

Question

我正在使用Arduino Mega来实现设备的矩形图案移动，这意味着每次到达结束边缘时长度和宽度都会减小。我已经编写了代码，但它无法正常工作

//servo header
#include <Servo.h>
#include <Math.h>
Servo myservo;//servo Object

//motor pin config
int m1r = 9;
int m1f = 10;
int m2f = 12;
int m2r = 11;


//infrared counter
int ir1val=0;//pulse count from irval
int ir2val=0;//pulse count from irval

int len=1;
int bred=2;

//Different sensors used 
int temp;
int buzz = 8;

const float radpulse = 0.05;//1 pulse = 0.05 meter
double pulsecount1;//pulse counter;
double pulsecount2;//pulse counter;
int pc1;//integer 
int pc2;

void setup() {

// initialize the Motor pin as an output:
pinMode(m1f, OUTPUT);
pinMode(m1r, OUTPUT);
pinMode(m2f, OUTPUT);
pinMode(m2r, OUTPUT);
pinMode(buzz,OUTPUT);

myservo.attach(A0);//BLDC motor attachment

Serial.begin(9600);// standard serial bandwidth of all modules 
}

void loop()
{   
pulsecount1=len/radpulse;
pc1=(int)round(pulsecount1);
pulsecount2=bred/radpulse;
pc2=(int)round(pulsecount2);
pc1=3;
pc2=2;
myservo.write(30);
delay(5000);

labelL1:

if((pc2 == 0)&&(pc1 == 0))
{
 goto Stop;
}

while(pc1 > ir1val)
{
  temp=((5.0*analogRead(A3)*100)/1024);
 if(temp >60)
  {
 //   goto hibernate;
  }
/*  if((analogread(A14) <= 300 || analogread(A14) >= 400) ||        (analogread(A15) <= 300) || analogread(A15) == 400))
  {               
    goto Stop;
  }*/
  digitalWrite(m2f,HIGH);
  digitalWrite(m1f,HIGH);
  //digitalWrite(m2f,HIGH);
  if(digitalRead(A2))
  {
    ir1val++;
  }
}
if(pc1 == ir1val)
{
  /*if((analogread(A14) <= 300 || analogread(A14) >= 400) || (analogread(A15) <= 300) || analogread(A15) == 400))
    {               
     goto Stop;
    }*/
    pc1--;
    ir1val=0;
    digitalWrite(m1f, HIGH);
    digitalWrite(m2f,LOW);
    delay(5000);
    goto labelB1;
}
labelB1:
while(pc2 != ir1val)
{
  temp=((5.0*analogRead(A3)*100)/1024);
  if(temp >60)
  {
 //   goto label2;
  }
/*  if((analogread(A14) <= 300 || analogread(A14) >= 400) || (analogread(A15) <= 300) || analogread(A15) == 400))
  {              
    goto Stop;
  }*/
  digitalWrite(m1f,HIGH);
  digitalWrite(m2f,HIGH);
  if(digitalRead(A2))
  {
    ir2val++;
  }
}
if(pc2 == ir2val)
{
  /*  if((analogread(A14) <= 300 || analogread(A14) >= 400) || (analogread(A15) <= 300) || analogread(A15) == 400))
  {               
    goto Stop;
  }*/
  pc2=pc2--;
  ir2val=0;
  digitalWrite(m1f, HIGH);
  digitalWrite(m2f,LOW);
  delay(5000);
  goto labelL1;
}
Stop:
  myservo.write(0);// stop servo
 digitalWrite(m1f,LOW);// stop motor
 digitalWrite(m2f,LOW);//stop motor
  digitalWrite(buzz,HIGH);//Buzzer
  delay(5000);
goto Stop;

/*
hibernate:
temp=((5.0*analogRead(A2)*100)/1024);
  if(temp >40)
  {
    delay(1000);
    goto hibernate;
  }
*/}