PID Program not working

Hello, I am new to programming as of this year and I have been working on a PD loop for the drive of our robot. The problem I am running into is that the robot will go to the first value that I put in, but it will not continue onto the next value.

#include “vex.h”

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/
/
/
You may want to perform some actions before the competition starts. /
/
Do them in the following function. You must return from this function /
/
or the autonomous and usercontrol tasks will not be started. This /
/
function is only called once after the V5 has been powered on and /
/
not every time that the robot is disabled. /
/
---------------------------------------------------------------------------*/

void preauton() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();}

//settiings

double kP=0.0309;
double kD=0.013;

double error =0.0;
double prevError = 0.0;
double derivative = 0.0;

bool running= true;

void drivePID(double target) {

while (running){
double LeftPosition=LeftEncoder.position(deg);
double RightPosition=RightEncoder.position(deg);
double averagePosition = (LeftPosition+RightPosition)/2;

  error = target-averagePosition;

  derivative= error-prevError;

  

  int lateralPower= kP*error + kD*derivative;

  FR.spin(fwd,lateralPower,voltageUnits::volt);
  BR.spin(fwd,lateralPower,voltageUnits::volt);
  FL.spin(fwd,lateralPower,voltageUnits::volt);
  BL.spin(fwd,lateralPower,voltageUnits::volt);

  Brain.Screen.print("error:");
  Brain.Screen.print(error);
  Brain.Screen.newLine();
  Brain.Screen.print(error);
  vex::task::sleep(10);

}

return ;}

// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …

/---------------------------------------------------------------------------/
/* /
/
Autonomous Task /
/
/
/
This task is used to control your robot during the autonomous phase of /
/
a VEX Competition. /
/
/
/
You must modify the code to add your own robot specific commands here. /
/
---------------------------------------------------------------------------*/

void autonomous(void) {
running=true;
drivePID(1000);
//reset encoders
RightEncoder.resetRotation();
LeftEncoder.resetRotation();

drivePID(300);

For future reference, please put you code in [code] [/code] tags

It looks like your while(running) loop might be stuck in an infinite loop because running is always true

You will have to make it so that running becomes false eventually. I think the best way of doing this is to make running false after error has been a sufficiently small value after a certain amount of time.

It would look something like this:

int timesWithinRange = 0;
//Set running to be true so that the loop runs
running = true;
while(running){
  double LeftPosition=LeftEncoder.position(deg);
  double RightPosition=RightEncoder.position(deg);
  double averagePosition = (LeftPosition+RightPosition)/2;


  error = target-averagePosition;

  derivative= error-prevError;

  

  int lateralPower= kP*error + kD*derivative;

  FR.spin(fwd,lateralPower,voltageUnits::volt);
  BR.spin(fwd,lateralPower,voltageUnits::volt);
  FL.spin(fwd,lateralPower,voltageUnits::volt);
  BL.spin(fwd,lateralPower,voltageUnits::volt);

  Brain.Screen.print("error:");
  Brain.Screen.print(error);
  Brain.Screen.newLine();
  Brain.Screen.print(error);
  if(std::abs(error) <= 2/*<<the error max value*/){
    timesWithinRange++;
  }
  else {
    timesWithinRange = 0;
  }
  running = timesWithinRange < 10; /* the amount of time (in loop iterations) that you want the error to be small */
  vex::task::sleep(10);

}
4 Likes

Hello,

Thanks for the help. It did solve the problem of the PID loop continuing onto the next command. However, at the end of commands the robot will turn randomly and I do not know why.

   #include "vex.h"
    #include <cmath>

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here
[//settiings

double kP=0.0309;
double kD=0.005;
double turnkP=0.09;
double turnkD= 0.007;

double error =0.0;
double prevError = 0.0;
double derivative = 0.0;

double TurnError=0.0;
double prevTurnError=0.0;
double TurnDerivative=0.0;







bool running= true;
 
void drivePID(double target,double turnTarget) {

      bool enabled=true;
      while(enabled){
        double LeftPosition=Left.position(deg);
        double RightPosition=Right.position(deg);
        double averagePosition = (LeftPosition+RightPosition)/2;
        error = target-averagePosition;
        derivative= error-prevError;
        int lateralPower= kP*error + kD*derivative;

        FR.spin(fwd,lateralPower,voltageUnits::volt);
        BR.spin(fwd,lateralPower,voltageUnits::volt);
        FL.spin(fwd,lateralPower,voltageUnits::volt);
        BL.spin(fwd,lateralPower,voltageUnits::volt);

      int TurnDifference = RightPosition-LeftPosition;

        TurnError= turnTarget - TurnDifference;
        TurnDerivative= TurnError-prevTurnError;
        
        int TurnMotorPower = (TurnError*turnkP + TurnDerivative*turnkD);

        FR.spin(fwd,lateralPower+TurnMotorPower,voltageUnits::volt);
        BR.spin(fwd,lateralPower+TurnMotorPower,voltageUnits::volt);
        FL.spin(fwd,lateralPower-TurnMotorPower,voltageUnits::volt);
        BL.spin(fwd,lateralPower-TurnMotorPower,voltageUnits::volt);

        
        
        Brain.Screen.print("error:");
        Brain.Screen.print(error);
       Brain.Screen.newLine();
        Brain.Screen.print("TurnError");
        Brain.Screen.print(TurnError);
        if (std::abs(error)<5) {
          break;
          
          enabled = false;      
        }
      }


    
      vex::task::sleep(10);

      return;
  

     


  
  }

  int reset(){
    Right.resetRotation();
    Left.resetRotation();
    return 0;}
  

   
   
   
    
        
      
    

  


void autonomous(void) {
  //task PIDTask = task(drivePID);

  running=true;

  
  
  
  ML.spinFor(reverse,0.5,rev);

  drivePID(2000,-480);
  
  reset();
  drivePID(2800,0);
  reset 
vex::task::sleep(1000);