我已经成功地控制我的电机位置使用PD控制器。但是我有一些编程相关的问题。
下面是我的代码：

#define encoder0PinA_M1  2
#define encoder0PinB_M1  22
int EnablePin = 8;
int PWMPin1 = 3;
int PWMPin2 = 11;

volatile signed int encoder0Pos = 0;
unsigned long LastTime;
signed int Input;
signed int Scaled_PID;
float PID_Output, Scaled_PID1;
signed int ErrorSum,ErrorDiff,Error,LastError;
float kp=6;
float ki=0;
float kd=1;
int SampleTime = 10;
int TimeChange;
unsigned long Now;

void setup()
{
  pinMode(encoder0PinA_M1, INPUT);
  //digitalWrite(encoder0PinA_M1, HIGH);      
  pinMode(encoder0PinB_M1, INPUT);
  pinMode(EnablePin, OUTPUT);
  pinMode(PWMPin1, OUTPUT);
  pinMode(PWMPin2, OUTPUT);
  //digitalWrite(encoder0PinB_M1, HIGH);      
  attachInterrupt(0, doEncoder, CHANGE);  
  Serial.begin (9600);
  Serial.println("start");                
}

void PID()
{
  Now = millis();
  TimeChange = Now - LastTime;
  if(TimeChange >= SampleTime)
  {
    Error =  Input - encoder0Pos;
    ErrorSum = ErrorSum + Error;
    ErrorDiff = Error - LastError;

    PID_Output = kp * Error + ki * ErrorSum + kd * ErrorDiff;

    LastError = Error;
    LastTime = Now;

  }
}

void speedlimitforward()
{
  if (PID_Output >= 15)
  {
    PID_Output= 15;
  }
  if(PID_Output <= -15)
  {
    PID_Output=-15;
  }
  Scaled_PID = PID_Output+15;
  digitalWrite(EnablePin, HIGH);
  analogWrite(PWMPin1,Scaled_PID);

}

void speedlimitbackward()
{
  if (PID_Output >= 20)
  {
    PID_Output= -20;
  }
  if(PID_Output <= -20)
  {
    PID_Output= 20;
  }
  Scaled_PID = PID_Output+20;
  digitalWrite(EnablePin, HIGH);
  analogWrite(PWMPin2,Scaled_PID);
}



void loop()
{
  Input=50;
  PID();
  speedlimitforward();
}

void doEncoder()
{
  if (digitalRead(encoder0PinA_M1) == digitalRead(encoder0PinB_M1))
  {
    encoder0Pos++;
  } else {
    encoder0Pos--;
  }

}
如果你看一下我的代码，我已经将Input声明为全局变量，并在loop（）中给予Input一个值（在这段代码中有50个计数）。此代码工作正常，电机在接近50个编码器计数时停止。
但是当我将上面代码中的循环更改为下面给出的代码时，我的电机不会移动。我希望它移动到50计数等待一段时间，然后返回到0计数：

void loop()
{
  Input=50;
  PID();
  speedlimitforward();

  delay(2000);
  Input=0;
  PID();
  speedlimitbackward();

  delay(2000);
  Input=-100;
  PID();
  speedlimitbackward();

 }