Buggy PID Controller

I am currently trying to program my first PID Controller. I finished programming all of the components of the controller, but when I run it, it will continuous run without stopping. After putting checks in, the PID controller exits the loop but the motors still run. I then tried to manually stop the motors but that resulted in the motors never starting.

float proportionalControl(float error){
  const float K_P = .2;
  float proportional = K_P * error;
  if(fabs(proportional) > 12){
    proportional = 12;
  }
  
  return proportional;
}

float integralControl(float buffer){
  const float K_I = .2;
  float integral = 0;
  if(buffer != 0){
    integral = K_I * buffer;
  }
  if(integral > 12){
    integral = 12;
  }
  else if(integral < -12){
    integral = -12;
  }
  return integral;
}

float derivativeControl(float error, float lastError){
  return error - lastError;
}


void pidControlForward(float distance) {
  //to calculate error, one needs a set point and a process variable
  //process variable tells the distance traveled
  //SET_POINT tells me the distance i need to travel (is a constant)
  //distance will be in inches, so I need to translate it into meters. 
  leftDrive.resetRotation();
  rightDrive.resetRotation();
  const float SET_POINT = distance;
  float processVariable = 0;
  //integral needs a buffer to store all past error. 
  float buffer = 0;
  float error = SET_POINT - processVariable;
  const float wheelCircumference = 12.59843;
  float lastError = 0;
  Brain.Screen.print("Pid Control begins! Inital Error: %f", error);
  while(error > 0.01){
    Brain.Screen.print("Entered loop!");
    
    if(processVariable / SET_POINT >= .7){
      buffer += error;
    }
    float voltageApplied = proportionalControl(error) + integralControl(buffer) + derivativeControl(error, lastError);
    if(voltageApplied > 12){
      voltageApplied = 12;
    }
    else if(voltageApplied < -12){
      voltageApplied = -12;
    }
    leftDrive.spin(fwd, voltageApplied, voltageUnits::volt);
    rightDrive.spin(fwd, voltageApplied, voltageUnits::volt);
    processVariable += leftDrive.rotation(rev) * wheelCircumference;
    lastError = error;
    error -= processVariable;
  }

  Brain.Screen.clearScreen();
  Brain.Screen.print("Error: %f, pv: ", error, processVariable);

  //Stop motors

}

I don’t know why your motors are buggy but you should probably have a wait(10, msec); or some type of delay in your while loop.

2 Likes

The incrementing of processVariable might be the issue. If this is supposed to represent the position of the robot, then it should just be the leftDrive.rotation(rev) * wheelCircumference on its own, instead of adding it with each iteration of the loop

1 Like