current error and prev error are the same in PID calculation

Projects Programming
1

This code should take the current error and make it the previous error for the next loop. they are always the exact same number. Any suggestions to fix this??

void PIDcalc() {

// float a = EstimateAngle() - INIT_ANGLE;
   float a = RwEst[1] - INIT_ANGLE;  
   angleBuffer[angleBufferIndex] = a;
    angleBufferIndex = (angleBufferIndex + 1) % ANGLE_BUFFER_LENGTH;
   
    float ang = GetAvgAngle();

    curErr = ang - INIT_ANGLE; //error   

    // Proprtional Term
  //  curErr = RwEst[1] - INIT_ANGLE; //error   
    
    // Integral Term
    SumErr += curErr;
    if (SumErr > SumErrMax) SumErr = SumErrMax;
    else if (SumErr < SumErrMin) SumErr = SumErrMin;
 //   integralTerm = SumErr * Ki / (Kp * elapsedTimeSec * 10.0);  // Ki*SumE/(Kp*Fs*X)
    integralTerm = SumErr * interval / 1000000 * Ki / Kp * 10.0;  // Ki*SumE/(Kp*Ts*X)   


    // Derivative Term
    derivativeTerm = curErr - prevErr;
     if(derivativeTerm > 0.1) derivativeTerm = 0.1;
    else if (derivativeTerm < -0.1) derivativeTerm = -0.1;
    derivativeTerm = derivativeTerm * Kd * interval / 1000000 / Kp;  // Kd(curErr-prevErr)*Ts/(Kp*X)
    if(elapsedTimeSec > 120) derivativeTerm = 120;
    else if (derivativeTerm < -120) derivativeTerm = -120;

    // PID Calc
    Cn = (curErr + integralTerm + derivativeTerm) * Kp / 10.0;
   // Serial.println (Cn);
    // Motor drive calc
    WheelDirection dir;
    if (Cn > 0) dir = DIR_FORWARD;
    else if (Cn < -0) dir = DIR_BACKWARD;
    else dir = DIR_STOP;
    throttle = abs(Cn);
    if (abs(ang) > 0.7)  MoveWheels(DIR_STOP, 0); //if angle too large to correct, stop motor       
    else MoveWheels(dir, throttle);

  prevErr = curErr;
2

Maybe you give us the whole code and tell us where you expect them to be different? Your last line reads:

prevErr = curErr;

so yes, I would always expect the two values to be the same.

3

ok, i attached the code. i combined 2 programs, they both had time functions and i have been switching between the 2 time calculations to get good data.

the attached code gets a current error, but should assign the current error from the last loop calc to the prev error for the next loop calc.

thanks for the help!

aramgp2.ino (13.5 KB)

4

From your posted code I would expect prevErr to be exactly the same as currErr when sending them to the serial line. Is prevErr staying at 0 while currErr changes? I don't get your problem yet...

5

If the system has achieved a steady state (either stationary or moving) those values should be the same. Either that or you lack resolution and you are unable to see the difference.

6

My resolution was too low. I multiplied the result by 10k and it showed up.

Thanks!