PID Not Stopping

#1

Hello! We tried adding PID to our autonomous but it does not stop the robot. The motor returns a proper encoder value, but the error is always only calculated once and never again.
Here is my code.

Auton Function:

    int moveBackward(int rotation, int velocity) {
  target=rotation;
  sensor=0;

  vex::task PID(calculation);

  while(error>5)
  {
    sensor=RFDrive.rotation(rotationUnits::deg);

    LFDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
    RFDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
    LBDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
    RBDrive.spin(vex::directionType::fwd,speed, vex::velocityUnits::pct);
  }

  LFDrive.stop(brakeType::coast);
  RFDrive.stop(brakeType::coast);
  LBDrive.stop(brakeType::coast);
  RBDrive.stop(brakeType::coast);
  vex::task::stop(PID);
  return 0;
}

PID:

#include "cmath"
#include"vex.h"

//the target value
int targetValue;
int targetSpeed;

//the value that is received by the sensor
double sensorValue;

//the integral
float integral;


//the big three constants
double kp = 0.5;
double ki = 0.2;
double kd = 0.1;

//sets the speed
float speed;
float error;
float previousError;
float derivative1;

int calculus()
{
  while (targetValue - sensorValue >5)
  {
    //error calculation
    error = targetValue - sensorValue;

    
    //integral calculation
    integral += error;

    
    //safety check
    if (error == 0 || error > targetValue)
    {
      integral = 0;
    }
    
    //safety check
    if (error > 1000)
    {
      integral = 0;

    }
    
    //derivative calculation
    derivative1 = error - previousError;

    
    //previous error calculation
     previousError = error;

          
    //final speed calculation
    speed = (kp * error) + (ki * integral) + (kd * derivative1);
    
    if(speed>targetSpeed)
    {
      speed=targetSpeed;
    }
    
    

    
    vex::task::sleep(15);
  }


  return 0;
}
#2

How does this even work?

  vex::task PID(calculation);

vs

int calculus()
{

i don’t know what the other code is but if error and speed are not initialized they are some random value.

1 Like
#3

Oh, ignore that, small mistake in the name. The function is the same as the task in the actual program

#4

Also, should I just the unassigned variables to 0 then? Would that fix the issue?

#5

Your calculation task needs a while (true) loop to keep it running simultaneously to the auton.

#6

well without the actual code i’m not sure what help i can provide. the other comment applied only if the function and task call were different.

typically the task would have some event that starts it from your auton. some pseudo code…

int PIDcalc() {
   // initial some things,
  while (1) {

     if (pid_go) {
      // pid stuff

        if ( near_setPoint) {
           pid_go = 0;
           // set output value to 0
        }
     }  else {
      // reset value specifically the output value to 0 so the motor does not turn
    }

    sleep ( some_time);
  } // end while
 return 1;
} // end task

// what ever code goes here for comp temp or other functions

void autonomous (void) {
  task PIDtask(PIDcalc);
  // setup PID setPoint
  pid_go=1;

  while (pid_go=1) {  // it may also be good to have a timeout task 
    sleep(some_time);
  }

  // setup PID setPoint
  pid_go=1;

  while (pid_go=1) {  // it may also be good to have a timeout task 
    sleep(some_time);
  }

  // and advance user might notice the redundancy in calling the task and make that a function
1 Like
#7

Yeah, this seemed to work. Thanks so much :smiley:

1 Like