Pages: [1]   Go Down
Author Topic: current error and prev error are the same in PID calculation  (Read 621 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 17
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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??

Code:
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;
Logged

Switzerland
Offline Offline
Faraday Member
**
Karma: 96
Posts: 4757
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Code:
prevErr = curErr;

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

Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 17
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.45 KB - downloaded 4 times.)
Logged

Switzerland
Offline Offline
Faraday Member
**
Karma: 96
Posts: 4757
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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...
Logged

South Texas
Offline Offline
Edison Member
*
Karma: 8
Posts: 1023
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 17
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Thanks!
Logged

Pages: [1]   Go Up
Jump to: