Go Down

Topic: Simple motor control with EasyPID.h (Read 654 times) previous topic - next topic

Robin2

Nov 16, 2017, 12:48 am Last Edit: Nov 18, 2017, 05:39 pm by Robin2
Following some comments below and some further thought I have very slightly amended the code and the example. Unless you are interested in reading the history from top to bottom I suggest you skip ahead to Reply #8

A while back I wanted to use the Arduino PID library to control the speed of a small DC motor in a model train but I could not figure out how to choose suitable K values. Eventually I figured out a simple program to control my motor using the core calculations from the PID library.

Recently I got a cheap 3D printer and while exploring how to control the extruder nozzle temperature I gained a lot more experience with PID control as the heater responds very slowly compared to the motor.

This has enabled me to encapsulate my code in the attached EasyPID.h file. I have also written the code so it only uses integer maths for better performance.

I hope my code may make it easier to incorporate PID control into projects, and maybe also make it easier to explore the internal workings of the system

As well as the attached motor control program I have successfully tested EasyPID.h with my 3D printer nozzle heater and a DIY servo in which a low geared DC motor adjusts the position of a potentiometer.


In the motor program there is a function called setupEasyPID() which provides a simple way to set the weightings (equivalent to the K values in the PID library).


To incorporate this into your own program just put a copy of the EasyPID.h file in the same folder as your .ino file and add this line near the top of your .ino file
Code: [Select]
#include "EasyPID.h"
and then include the function setupEasyPID() in your .ino file and call it from setup().



For convenience this is the code in EasyPID.h
Code: [Select]

// data and code for EasyPID

#include <Arduino.h>

class EasyPID {

    public:
            // the control variables
        long errorRange;
        long outputRange;
        
        int errorOutOfRangeSlowOutput;
        int errorOutOfRangeFastOutput;
        
        int iTermWeightingSlow;
        int pTermWeightingSlow;
        int dTermWeightingSlow;
        
        int iTermWeightingFast;
        int pTermWeightingFast;
        int dTermWeightingFast;

    // private: // having these public is easier for debugging
                // but they would not normally need to accessed by the user's program XXXXX
    
            // variables internal to the function
        long scaledError ;
        long scaledErrorRange;
        long scaledOutputRange;
        long totalOutput;
        
        int iTermWeighting;
        int pTermWeighting;
        int dTermWeighting;
        
        long unweightedErrorOutput;
        long prevUnweightedErrorOutput = 0;
        
        byte precisionScaleFactor = 10;
        
        long oorTerm;
        long iTerm;
        long iTermChange;
        long eTerm;
        long dTerm;



//==============

    public:
        long calc(long errVal) {

            // scale up the values to retain precision during calculations
        scaledError = errVal << precisionScaleFactor;
        scaledErrorRange = errorRange << precisionScaleFactor;
        scaledOutputRange = outputRange << precisionScaleFactor;
        
            // if the error is outside the range for EasyPID control
            //    use the special values and not EasyPID control
        if (scaledError > scaledErrorRange) {
            oorTerm = errorOutOfRangeSlowOutput;
            iTerm = 0;
            eTerm = 0;
        }
        else if (scaledError < -scaledErrorRange) {
            oorTerm = errorOutOfRangeFastOutput;
            iTerm = 0;
            eTerm = 0;
        }

            // if the error is within the range apply EasyPID
        else {
            oorTerm = 0;
                // use different weghtings for positive or negative errors
            if (scaledError >= 0) {
                iTermWeighting = iTermWeightingSlow;
                pTermWeighting = pTermWeightingSlow;
                dTermWeighting = dTermWeightingSlow;
            }
            else {
                iTermWeighting = iTermWeightingFast;
                pTermWeighting = pTermWeightingFast;
                dTermWeighting = dTermWeightingFast;
            }
                // now we are ready for the EasyPID calcs
                
                // calculate the unweighted error output for the current error
            if (scaledError >= 0) {
                unweightedErrorOutput = scaledError / errorRange;
            }
            else {
                    // this avoids problems with division of a negative integer
                unweightedErrorOutput = -(abs(scaledError) / errorRange);
            }

                // update the iTerm
            iTermChange = unweightedErrorOutput * iTermWeighting;
            iTerm += iTermChange;
            
            if (iTerm > scaledOutputRange) {
                iTerm = scaledOutputRange;
            }
            if (iTerm < -scaledOutputRange) {
                iTerm = -scaledOutputRange;
            }

                // calculate the eTerm
            eTerm = unweightedErrorOutput * pTermWeighting;
            
                // calculate the dTerm
            dTerm = (unweightedErrorOutput - prevUnweightedErrorOutput) * dTermWeighting;
            prevUnweightedErrorOutput = unweightedErrorOutput;
        }

        totalOutput = iTerm + eTerm + dTerm;
        
            // scale it back to real-world values and add oorTerm
        totalOutput = (totalOutput >> precisionScaleFactor) + oorTerm;
        
        return totalOutput;
    }

//=================
    public:
        void printKeyVars() {
            Serial.print("  scaledError "); Serial.print(scaledError);
            Serial.print(" unweightedErrorOutput "); Serial.print(unweightedErrorOutput);
            Serial.print(" oorTerm "); Serial.print(oorTerm);
            Serial.print(" iTerm "); Serial.print(iTerm);
            Serial.print(" eTerm "); Serial.print(eTerm);
            Serial.print(" dTerm "); Serial.print(dTerm);
            Serial.println();
        }
};


And this is the code for the function setupEasyPID() as it appears in the attached EasyPIDMotorDemo.ino
Code: [Select]

void setupEasyPID() {
    myEasyPID.errorRange = targetMicros;
    myEasyPID.outputRange = 255;

        // values to use when the error is out of range
    myEasyPID.errorOutOfRangeSlowOutput = 255;
    myEasyPID.errorOutOfRangeFastOutput = 0;

        // weightings to use when speed is below target
        //  values should be in range 0 - 255 - for example 32 represents 12.5%
    myEasyPID.iTermWeightingSlow = 32; // equivalent to kI
    myEasyPID.pTermWeightingSlow = 32; // equivalent to kP
    myEasyPID.dTermWeightingSlow = 0;  // equivalent to kD

        // and when speed is above target (in this case they are the same)
    myEasyPID.iTermWeightingFast = 32;
    myEasyPID.pTermWeightingFast = 32;
    myEasyPID.dTermWeightingFast = 0;

}



Have fun.

As usual, comments are welcome.
Two or three hours spent thinking and reading documentation solves most programming problems.

zhomeslice

#1
Nov 17, 2017, 04:19 am Last Edit: Nov 17, 2017, 04:19 am by zhomeslice
It's great to see this code become refined!!! I'm excited to work with it more.  I'm curious how you compensate (or not) for different sampling rates. I didn't see any duration calculations.
The motor sampling rate varies depending upon what RPM your desiring to control and so the influence KI and Kd have to change accordingly.
your temperature control you mentioned for your extruder is unclear. do you sample at the highest rate possible hand have the factors tuned to that or do you sample at a much slower rate where the change in temperature could be larger and become a stronger influence.

Z
HC

Robin2

It's great to see this code become refined!!! I'm excited to work with it more.  I'm curious how you compensate (or not) for different sampling rates. I didn't see any duration calculations.
The motor sampling rate varies depending upon what RPM your desiring to control and so the influence KI and Kd have to change accordingly.
There is a degree of mystery (to me) in all of this. For the motor the errorRange is the sample period so I think there is automatic compensation which allows the K values to work unchanged.


Quote
your temperature control you mentioned for your extruder is unclear. do you sample at the highest rate possible hand have the factors tuned to that or do you sample at a much slower rate where the change in temperature could be larger and become a stronger influence.
I had debated posting a much longer treatise (I had actually written it) and decided brevity is best.

I found that about 5 seconds is a suitable sample interval (and PID calculation interval) for the 3D printer nozzle heater. That seems to be the time period within which the temperature can change by 1°C when near the target temperature. And I am restricting the PID calcs so that they only apply within +/- 20°C of the target.

I also created a crude DIY servo using a low-geared DC motor to turn a potentiometer and that seems to work well (within the limitations of the hardware) with a sample interval of 50 millisecs.

If you would be prepared to try this code on your balancing robot (if you still have it) I would very much appreciate it.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

zhomeslice

There is a degree of mystery (to me) in all of this. For the motor the errorRange is the sample period so I think there is automatic compensation which allows the K values to work unchanged.
I worked the older version of the code with the Tachometer project a while back and discovered I could tune the Non specific time based routine to a general range of RPM's but once I exceeded a specific setpoint the Integral and Derivative influence exceeded a controllable range and had to be retuned for the higher sample rate. But with that said 90% of the time the sample rate is fixed somewhere else in the code and this becomes irrelevant. So this Integer based PID code becomes valuable just for its processing speed and simplicity. 

Quote
I had debated posting a much longer treatise (I had actually written it) and decided brevity is best.

I found that about 5 seconds is a suitable sample interval (and PID calculation interval) for the 3D printer nozzle heater. That seems to be the time period within which the temperature can change by 1°C when near the target temperature. And I am restricting the PID calcs so that they only apply within +/- 20°C of the target.
So here's an experiment that could be tried. You have a sample rate of 5 and I think you could control with a faster sample rate of 1 second. Kp shouldn't change. Ki I believe should be 5 times to large to control at a 1 second rate so divide Ki by 5. Kd on the other hand should be 5 times to small when changing to a 1 second interval so multiply Kd by 5.  
With that in mind you could pre calculate you Kd and Ki values to use a 1 second interval as a standard so that faster/slower terval could be made elsewhere and your PID code would be able to maintain control. 

Quote
I also created a crude DIY servo using a low-geared DC motor to turn a potentiometer and that seems to work well (within the limitations of the hardware) with a sample interval of 50 millisecs.

If you would be prepared to try this code on your balancing robot (if you still have it) I would very much appreciate it.

...R
I'm already planning on integrating the code into my balancing bot for testing :)
Z
HC

Robin2

I'm already planning on integrating the code into my balancing bot for testing :)
That would be great. Thanks.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

zhomeslice

#5
Nov 18, 2017, 06:53 am Last Edit: Nov 18, 2017, 06:55 am by zhomeslice
That would be great. Thanks.

...R
This is my initial interpretation of how to implement your code with my bot. I plan on testing more tomorrow but I would like to get your thoughts on my implementation and desired calculations I documented as notes in my code.
Balancing bot initial Test code
Code: [Select]
#include "EasyPID.h"
int HBridgeEnablePWM = 5;
int ForwardEnable = 6;
int ReverseEnable = 7;
unsigned long TimingInterval = 10;
int Deadband = 34; // Point at which the motors start moving either forward or reverse.
long setPoint = 0; // balanced at Zero Degerees
EasyPID myEasyPID;
void setup() {
  Serial.begin(115200);

  pinMode(HBridgeEnablePWM, OUTPUT);
  pinMode(ForwardEnable, OUTPUT);
  pinMode(ReverseEnable, OUTPUT);
  analogWrite(HBridgeEnablePWM, 0);
  digitalWrite(ForwardEnable, LOW);
  digitalWrite(ReverseEnable, LOW);
  setupEasyPID();

}

void setupEasyPID() {
  myEasyPID.errorRange = 0;
  myEasyPID.outputRange = ((255 - Deadband) * 2) * 100;
  // PWM = 0 - 255 but the deadband the motors do nothing so subtract that. it will be added back in later.
  // Balancing bot has 2 ways to fall so we can go deadband to 255 in both forward and backward directions so our range is multiplied by 2.
  // Lets get a decimal point of percision to allow for more detailed math later on when applying turning and other corrections with the differences in the motors power.


  // values to use when the error is out of range
  myEasyPID.errorOutOfRangeSlowOutput = myEasyPID.outputRange * 0.5; // we only need Half the range to be positive (falling forward)
  myEasyPID.errorOutOfRangeFastOutput = -1 * myEasyPID.errorOutOfRangeSlowOutput; // and the other half to be negitive (falling Backwads)

  // weightings to use when speed is below target
  //  values should be in range 0 - 255 - for example 32 represents 12.5%

  //   Im converting these into 10ms ranges
  myEasyPID.iTermWeightingSlow = 100; // equivalent to kI // Staus at 100 in otherwords when the bot is off by 1 degree the PID output is at  100
  myEasyPID.pTermWeightingSlow = 0; // equivalent to kP // because the balancing bot setpoint is zero and at zero the motors should be off Ki is irrellivent for Level  surface balancing
  myEasyPID.dTermWeightingSlow = 4;  // equivalent to kD // This is equivilant to for every 10° in change over the 10ms duration add 40 to the PWM poutput which has a range of +-25500.
  // dificulties arrise when the bot may change angle at hundreds of degrees a second and this is multiplied by 4.

  // These are my thouts on this.

  // and when speed is above target (in this case they are the same)
  myEasyPID.iTermWeightingFast = 100;
  myEasyPID.pTermWeightingFast = 0;
  myEasyPID.dTermWeightingFast = 4;

}

void loop() {
  long Power;
  long angle;
  static long ShiftedAngle = 1; // test angle
  // put your main code here, to run repeatedly:
  static unsigned long _ETimer;
  if ( millis() - _ETimer >= (TimingInterval)) {
    _ETimer += (TimingInterval);

    angle = 10; // This will receive the angle from the MPU6050 and 10 is just for testing
    //with a fixed angle and my deadband at 34 the output should be at 44
    // I have Integral off so there is no windup.
    // in actual life the rate of fall when the balancing bot got to 10 degrees would be kicking the derivative in and at this angle and the output would spike to nearly 150+ out of 255 to catch the fall
   
    long AngleFromSetpoint = angle - setPoint;
    long Output = myEasyPID.calc(AngleFromSetpoint);
    if (Output > 0) {
      digitalWrite(ForwardEnable, HIGH);
      digitalWrite(ReverseEnable, LOW);
      Power = constrain(abs(Output) * .01 + Deadband, 0, 255);
      analogWrite(HBridgeEnablePWM, Power);
    }
    else if (Output > 0) {
      digitalWrite(ForwardEnable, LOW);
      digitalWrite(ReverseEnable, HIGH);
      Power = constrain(abs(Output) * .01 + Deadband, 0, 255);
      analogWrite(HBridgeEnablePWM, Power);
    }
    else {
      digitalWrite(ForwardEnable, LOW);
      digitalWrite(ReverseEnable, LOW);
      analogWrite(HBridgeEnablePWM, 0); // COAST
    }
    Power = constrain(abs(Output) * .01 + Deadband, 0, 255);
    analogWrite(HBridgeEnablePWM, Power);
  }
}

If this is good I will shift it into my basic balancing bot code that works replacing PID_v1 (Revised) for testing in my actual bot.
Z
HC

Robin2

#6
Nov 18, 2017, 10:14 am Last Edit: Nov 18, 2017, 11:19 am by Robin2
I just have comments on these two lines
Code: [Select]
myEasyPID.errorRange = 0;
  myEasyPID.outputRange = ((255 - Deadband) * 2) * 100;


If the errorRange is 0 then the pid calcs will never be used. They are only applied to error values that are in the range errorRange to -errorRange.

You will also run into division by 0 errors!  

The errorRange is used to standardize the error value so that the same code can work when the error is in degrees or in thousands of microsecs. If your Robot should operate within +/- 45° of vertical then I suggest you set 45 as the error range. Or maybe only 22 - and allow the out of range values to jerk it upright outside that range.


I think your reference to the output range has identified a weakness in my code. I think it only works in the range +/- 255. Let me think more about this.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

Robin2

#7
Nov 18, 2017, 05:32 pm Last Edit: Nov 18, 2017, 05:43 pm by Robin2
OK. I've been thinking some more about this. Let's see if I can explain it clearly. Interestingly, while I had not given sufficient thought to the question of the outputRange (having been focused on other parts of the system), the code as is actually works.

The value in the outputRange variable acts as a cap on the value of the iTerm. So a bigger outputRange implies a bigger max for the iTerm. Seems simple.

But, the value of the iWeighting (or the kI value in old money) is inextricably linked with the outputRange. Think of it like this. If the error is at the max of the errorRange and if the iWeighting is 1 (one) then the iTerm will increment by 1 on each iteration. Thus, if the outputRange is 255 it will require 255 iterations for the iTerm to reach the max and if the outputRange is 1023 it will take 1023 iterations to reach the max. (Assuming of course that the error does not change).

That implies that if you want a fast responding system that gets the iTerm to the max in (say) 10 iterations and if you have an outputRange of 255 then you will need an iWeighting of about 26 and if you have an outputRange of 1023 you will need an iWeighting of about 102.

Or, put another way, the .Weighting value is always a fraction of the ouputRange.


That led me on to reconsider the question of the resolution of the system - the smallest variation it can work with. That is determined by the precisionScaleFactor which I have set at 10 (meaning 210 or 1024) in the code in my Original Post which means the smallest variation is 0.1%. When I looked more closely at the motor control numbers I realized that at a slow motor speed of 20,000 µsecs per revolution 0.1% would represent an error of 200. In other words any error less than 200 would not cause any change to the system. I think the system is able to control the speed more precisely than that so, to be on the safe side I think it would be sensible to increase the precisionScaleFactor to 12 (meaning 4096) which will allow errors down to about 0.025% to be recognized.


I also realized that there is no need to scale up the errorRange as it is not used in the main calculations.

And I have added a few lines of code to cap the totalOutput value at the outputRange


Rather than possibly cause confusion by amending my Original Post I am creating an updated version after this. And the code in the Original Post will work properly - the new version is just a minor refinement.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

Robin2

#8
Nov 18, 2017, 05:36 pm Last Edit: Nov 18, 2017, 05:40 pm by Robin2
This is essentially the same as in my Original Post but with a few updates as mentioned in Reply #7. Thanks to @zhomeslice for his comments

A while back I wanted to use the Arduino PID library to control the speed of a small DC motor in a model train but I could not figure out how to choose suitable K values. Eventually I figured out a simple program to control my motor using the core calculations from the PID library.

Recently I got a cheap 3D printer and while exploring how to control the extruder nozzle temperature I gained a lot more experience with PID control as the heater responds very slowly compared to the motor.

This has enabled me to encapsulate my code in the attached EasyPID.h file. I have also written the code so it only uses integer maths for better performance.

I hope my code may make it easier to incorporate PID control into projects, and maybe also make it easier to explore the internal workings of the system

As well as the attached motor control program I have successfully tested EasyPID.h with my 3D printer nozzle heater and a DIY servo in which a low geared DC motor adjusts the position of a potentiometer.


In the motor program there is a function called setupEasyPID() which provides a simple way to set the weightings (equivalent to the K values in the PID library).


To incorporate this into your own program just put a copy of the EasyPID.h file in the same folder as your .ino file and add this line near the top of your .ino file
Code: [Select]
#include "EasyPID.h"
and then include the function setupEasyPID() in your .ino file and call it from setup().



For convenience this is the code in EasyPID.h
Code: [Select]

// data and code for EasyPID

#include <Arduino.h>

class EasyPID {

 public:
 // the control variables
 long errorRange;
 long outputRange;
 
 int errorOutOfRangeSlowOutput;
 int errorOutOfRangeFastOutput;
 
 int iTermWeightingSlow;
 int pTermWeightingSlow;
 int dTermWeightingSlow;
 
 int iTermWeightingFast;
 int pTermWeightingFast;
 int dTermWeightingFast;

 // private: // having these public is easier for debugging
 // but they would not normally need to accessed by the user's program XXXXX
 
 // variables internal to the function
 long scaledError ;
 long scaledOutputRange;
 long totalOutput;
 
 int iTermWeighting;
 int pTermWeighting;
 int dTermWeighting;
 
 long unweightedErrorOutput;
 long prevUnweightedErrorOutput = 0;
 
 byte precisionScaleFactor = 12;
 
 long oorTerm;
 long iTerm;
 long iTermChange;
 long eTerm;
 long dTerm;



//==============

 public:
 long calc(long errVal) {

 // scale up the values to retain precision during calculations
 scaledError = errVal << precisionScaleFactor;;
 scaledOutputRange = outputRange << precisionScaleFactor;
 
 // if the error is outside the range for EasyPID control
 //    use the special values and not EasyPID control
 if (errVal > errorRange) {
 oorTerm = errorOutOfRangeSlowOutput;
 iTerm = 0;
 eTerm = 0;
 }
 else if (errVal < -errorRange) {
 oorTerm = errorOutOfRangeFastOutput;
 iTerm = 0;
 eTerm = 0;
 }

 // if the error is within the range apply EasyPID
 else {
 oorTerm = 0;
 // use different weghtings for positive or negative errors
 if (scaledError >= 0) {
 iTermWeighting = iTermWeightingSlow;
 pTermWeighting = pTermWeightingSlow;
 dTermWeighting = dTermWeightingSlow;
 }
 else {
 iTermWeighting = iTermWeightingFast;
 pTermWeighting = pTermWeightingFast;
 dTermWeighting = dTermWeightingFast;
 }
 // now we are ready for the EasyPID calcs
 
 // calculate the unweighted error output for the current error
 if (scaledError >= 0) {
 unweightedErrorOutput = scaledError / errorRange;
 }
 else {
 // this avoids problems with division of a negative integer
 unweightedErrorOutput = -(abs(scaledError) / errorRange);
 }

 // update the iTerm
 iTermChange = unweightedErrorOutput * iTermWeighting;
 iTerm += iTermChange;
 
 if (iTerm > scaledOutputRange) {
 iTerm = scaledOutputRange;
 }
 if (iTerm < -scaledOutputRange) {
 iTerm = -scaledOutputRange;
 }

 // calculate the eTerm
 eTerm = unweightedErrorOutput * pTermWeighting;
 
 // calculate the dTerm
 dTerm = (unweightedErrorOutput - prevUnweightedErrorOutput) * dTermWeighting;
 prevUnweightedErrorOutput = unweightedErrorOutput;
 }

 totalOutput = iTerm + eTerm + dTerm;
 
 // scale it back to real-world values and add oorTerm
 totalOutput = (totalOutput >> precisionScaleFactor) + oorTerm;
 
 if (totalOutput > outputRange) {
 totalOutput = outputRange;
 }
 if (totalOutput < -outputRange) {
 totalOutput = -outputRange;
 }
 
 return totalOutput;
 }

//=================
 public:
 void printKeyVars() {
 Serial.print("  scaledError "); Serial.print(scaledError);
 Serial.print(" unweightedErrorOutput "); Serial.print(unweightedErrorOutput);
 Serial.print(" oorTerm "); Serial.print(oorTerm);
 Serial.print(" iTerm "); Serial.print(iTerm);
 Serial.print(" eTerm "); Serial.print(eTerm);
 Serial.print(" dTerm "); Serial.print(dTerm);
 Serial.println();
 }
};


And this is the code for the function setupEasyPID() as it appears in the attached EasyPIDMotorDemo.ino
Code: [Select]

void setupEasyPID() {
 myEasyPID.errorRange = targetMicros;
 myEasyPID.outputRange = 255;

 // values to use when the error is out of range
 myEasyPID.errorOutOfRangeSlowOutput = 255;
 myEasyPID.errorOutOfRangeFastOutput = 0;

 // weightings to use when speed is below target
 //  values should be in range 0 - outputRange
        //   for example 32 represents 12.5% if outputRange is 255
 myEasyPID.iTermWeightingSlow = 32; // equivalent to kI
 myEasyPID.pTermWeightingSlow = 32; // equivalent to kP
 myEasyPID.dTermWeightingSlow = 0;  // equivalent to kD

 // and when speed is above target (in this case they are the same)
 myEasyPID.iTermWeightingFast = 32;
 myEasyPID.pTermWeightingFast = 32;
 myEasyPID.dTermWeightingFast = 0;

}



Have fun.

As usual, comments are welcome.
Two or three hours spent thinking and reading documentation solves most programming problems.

zhomeslice

Is the following code there because the timing changes the  reaction when we are above setpoint compared to below?

I'm not having the luck I need getting the balancing bot to react properly so I'm trying to simplify the PID code you provided to have 1 set of terms and 

Help me understand your purpose in this code.
Code: [Select]
if (scaledError >= 0) {
 iTermWeighting = iTermWeightingSlow;
 pTermWeighting = pTermWeightingSlow;
 dTermWeighting = dTermWeightingSlow;
 }
 else {
 iTermWeighting = iTermWeightingFast;
 pTermWeighting = pTermWeightingFast;
 dTermWeighting = dTermWeightingFast;
 }

Z
HC

Robin2

#10
Nov 21, 2017, 10:07 am Last Edit: Nov 21, 2017, 10:09 am by Robin2
Is the following code there because the timing changes the  reaction when we are above setpoint compared to below?
No. It is just there because some systems respond differently on different sides of the setpoint. For example my 3D printer nozzle heater responds differently when cooling compared to heating - heating is forced, cooling is natural. I found that a big drop in the PWM value for the heater was needed to get the heating effect out of the way and allow cooling to take place.

For your robot I would expect both sets of values to be the same as the motor provides the same corrective influence in both directions.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

zhomeslice

Discovered, I think one errors with your code. 

The error was with the calculation of the error from setpoint itself,
If we are below setpoint the error must be negative.
Placed lots of notes in the code :)
Code: [Select]
// calculate the unweighted error output for the current error
if (scaledError < 0) { // this is the same as the abs() function so im testing this here and only once speeds up code
// this avoids problems with division of a negative integer
unweightedErrorOutput = -scaledError / errorRange; // Since we know the value is negative and we only want positive make it positive
unweightedErrorOutput = -unweightedErrorOutput; // our unweighted Error must be negative if it is below setpoint
} else {
unweightedErrorOutput = scaledError / errorRange;
}


I also simplified the setup and removed (alternative control) code to get down to the basics of PID
I used the constrain macro to easily limit the output and because it is efficient
I also changed the code with the abs() function because we were basically doing the same test twice.

I'm excited to try this out with several of my projects. 
I've added notes of my changes. 

Code: [Select]
#define DEBUG
#ifdef DEBUG
#define DPRINTSTIMER(t)    for (static uint32_t SpamTimer; (uint32_t)(millis() - SpamTimer) >= (t); SpamTimer = millis())
#define DPRINTLN(...)      Serial.println(__VA_ARGS__)
#define DPRINTSFN(StrSize,Name,...) {char S[StrSize];Serial.print(F("\t"));Serial.print(F(Name));Serial.print(F(" ")); Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Precision

#else
#define DPRINTSTIMER(t)    if(false)
#define DPRINTLN(...)      //blank line
#define DPRINTSFN(...)     //blank line
#endif

// data and code for EasyPIDZ

// Modifications make to attempt to control my balancing bot.


// used constrain to simplify limits in the code here are the macros for constrain and MIN and MAX
//#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
//#define MIN(a,b) (((a)<(b))?(a):(b))
//#define MAX(a,b) (((a)>(b))?(a):(b))

#include <Arduino.h>

class EasyPID {

public:
// the control variables
long errorRange;
long outputRangeLow;
long outputRangeHigh;

int errorOutOfRangeSlowOutput;
int errorOutOfRangeFastOutput;

long scaledError ;
long scaledOutputRangeLow;
long scaledOutputRangeHigh;
long totalOutput;

int iTermWeighting;
int pTermWeighting;
int dTermWeighting;

long unweightedErrorOutput;
long prevUnweightedErrorOutput = 0;

byte precisionScaleFactor = 12;

long oorTerm;
long iTerm;
long iTermChange;
long eTerm;
long dTerm;



//==============

public:

void Initialize(int pTerm, int iTerm, int dTerm, byte Direction, long InputRange, long OutputLow, long OutputHigh){

Direction = (Direction <= -1)?-1:1;
iTermWeighting = Direction * iTerm;
pTermWeighting = Direction * pTerm;
dTermWeighting = Direction * dTerm;
errorRange = InputRange;
outputRangeLow = OutputLow;
outputRangeHigh = OutputHigh;

// lets pre calculate this here
scaledOutputRangeLow = outputRangeLow << precisionScaleFactor;
scaledOutputRangeHigh = outputRangeHigh << precisionScaleFactor;
}

// i need a way to clear the windup as my balancing bot is almost at zero when i start the calc loop. if the windup is at extremes then full poser is applied and the bot will crash.
void ItermWindupReset(){
iTerm = 0;
}


long calc(long errVal) {

// scale up the values to retain precision during calculations
scaledError = errVal << precisionScaleFactor;
scaledError = constrain(scaledError, -errorRange, errorRange);// if our error from setpoint is outside of our desired extents Lock the error at extents

// calculate the unweighted error output for the current error
if (scaledError < 0) { // this is the same as the abs() function so testing this here and only once speeds up code
// this avoids problems with division of a negative integer
unweightedErrorOutput = -scaledError / errorRange; // Since we know the value is negative and we only want positive make it positive
unweightedErrorOutput = -unweightedErrorOutput; // our unweighted Error must be negative if it is below setpoint
} else {
unweightedErrorOutput = scaledError / errorRange;
}

// calculate the eTerm
eTerm = unweightedErrorOutput * pTermWeighting;

// update the iTerm
// iTerm += unweightedErrorOutput *  DeltaTS * (iTermWeighting * (double)controllerDirection); // Integral term using floating point
// by eliminating the DeltaT in seconds itremWeighting is fixed with the sample duration DeltaTS calculated in as part of its value

iTermChange = unweightedErrorOutput * iTermWeighting;
iTerm += iTermChange;
iTerm = constrain(iTerm, scaledOutputRangeLow, scaledOutputRangeHigh);

// update the dTerm
dTerm = (unweightedErrorOutput - prevUnweightedErrorOutput) * dTermWeighting;
prevUnweightedErrorOutput = unweightedErrorOutput;

// add it all up
totalOutput = eTerm + iTerm + dTerm;

// scale it back to real-world values
long totalOutputZ; // needed to add for testing purposes Z
totalOutputZ = (totalOutput >> precisionScaleFactor);
totalOutputZ = constrain(totalOutputZ, outputRangeLow, outputRangeHigh);
// Debugging

DPRINTSTIMER(100){ // Spam Timer Macro 100ms delay between outputs.
DPRINTSFN(10,"errVal",errVal,6,2);
DPRINTSFN(10,"scaledError",scaledError,6,2);
DPRINTSFN(10,"eTerm",eTerm,6,2);
DPRINTSFN(10,"iTermChange",iTermChange,6,2);
DPRINTSFN(10,"iTerm",iTerm,6,2);
DPRINTSFN(10,"dTerm",dTerm,6,2);
DPRINTSFN(10,"totalOutput",totalOutput,6,2);
DPRINTSFN(10,"totalOutputZ",totalOutputZ,6,2);
DPRINTLN();
}

return totalOutputZ;
}
};

My changes are allowing me to gain a solid grasp on how your integer code is working. I think it is going to be excellent when I get it working :)

Thanks for your efforts. This way of calculating PID is incredibly fast!
Z
HC

Robin2

Your style changes are confusing me :)  :)   (but each to his own).

I have taken a copy of your version and I will try to convert it back to my simple style so I can see if you have made any changes of substance.

Thank you very much for your interest and effort.

...R
Two or three hours spent thinking and reading documentation solves most programming problems.

Robin2

#13
Nov 21, 2017, 04:30 pm Last Edit: Nov 21, 2017, 04:46 pm by Robin2
Perhaps you can confirm that my understanding (in green) of what you have done is correct ...

Rather than special treatment when the error is outside the range you are forcing it to stay within the range.
I'm not sure if that is essential for your balancer, but it would not be suitable for my heater because I want things to happen more quickly when it is outside the range.

My strong preference is to deal with that sort of special case before the error value is passed to EasyPID rather than adapt EasyPID to deal with special cases.

And maybe I should apply the same logic to my own special cases :) which would make EasyPID simpler


You seem to be allowing for different high and low range limits?
Does your project need different values? If so, why?


I'm not sure what problem your ItermWindupReset() function is intended to solve. Your comment says "if the windup is at extremes ..." How could it get to an extreme between one iteration and the next?  I wonder is it the same problem that led me to having different behaviour outside the range. At startup, with my system (for example) the heater heats up with half power (or whatever) until it gets within range. Then the PID takes over with the Iterm at zero.

What is the range of values that your system is producing and from which the error is calculated?

And I agree that my use of abs() is unnecessary - that's what comes from countless revisions and not seeing the trees for the wood.


...R
Two or three hours spent thinking and reading documentation solves most programming problems.

zhomeslice

#14
Nov 22, 2017, 03:34 am Last Edit: Nov 22, 2017, 07:10 am by zhomeslice
Perhaps you can confirm that my understanding (in green) of what you have done is correct ...

Rather than special treatment when the error is outside the range you are forcing it to stay within the range.
I'm not sure if that is essential for your balancer, but it would not be suitable for my heater because I want things to happen more quickly when it is outside the range.
with your original code when you were outside the limits you did something else.  while My Code  isn't necessary (Kp may go to the extreme and Ki winds up faster), My concern was more for how to prevent overflows with the precision shifted and long ints. If you don't see this as an issue having the error value outside a reasonable range control band isn't an issue.
For example if my temperature is at 23°C  and you want to control around 270°C for the filament melting point (just a guess)  even if you limited the input temperature between +-30°C into the PID loop your error of 30° below setpoint would cause the output to ramp to full (outputRangeHigh) due to the integral windup within a reasonable time.  The proportional control shouldn't influence the startup that much.

Quote
My strong preference is to deal with that sort of special case before the error value is passed to EasyPID rather than adapt EasyPID to deal with special cases.

And maybe I should apply the same logic to my own special cases :) which would make EasyPID simpler
Simplifying this helped me understand the core  and function of the code.
My biggest concerns is that the Kd multiplier can't reach the fractional values I may need to get the balancing bot to control.

Quote
You seem to be allowing for different high and low range limits?
Does your project need different values? If so, why?
for my balancing bot i need a +- 2550 int( this with a precision of .1  abs(+-255) -> PWM) the Positive values are for forward negative values are for reverse. My Reflow oven I need just 0-2000 milliseconds using a solid state relay to pulse the heating elements between full on to nothing with a pulse width of 2 seconds. on my incubator project using 60Hz dimming I need a range from 0 ~ 950.  Actual input range is from 0 ~1024 but the last part isn't used as the dimmer is on full at about 950 (Timer 2 PWM). so limiting the integral windup and the output in the PID is critical to maintain proper control.

Quote
I'm not sure what problem your ItermWindupReset() function is intended to solve. Your comment says "if the windup is at extremes ..." How could it get to an extreme between one iteration and the next?  I wonder is it the same problem that led me to having different behaviour outside the range. At startup, with my system (for example) the heater heats up with half power (or whatever) until it gets within range. Then the PID takes over with the Iterm at zero.
For my balancing bot I must reset the integral iTerm to zero just before starting and giving control to the PID. this way the output starts at zero without any windup The output range for the Balancing bot is  +-2550 (+-255.0) after shifting by 10 later on.
To answer the question: "How could it get to an extreme between one iteration and the next? " the reset is only needed at startup if any prior iTerm calculations were leftover from my bot losing control and  falling over I need to get rid of them before giving control back to the PID.


Concerning your Heater:
How I would capture this is similar to my Reflow oven. I have a warmup setpoint below the actual desired setpoint.  because the temperature error at its max the integral will windup. With the integral all wound up the heating element will cause the temperature to overshoot the preheat setpoint causing the integral to start unwinding. Have this preheat setpoint so with full windup it doesn't overshoot the desired setpoint just gets close. Ideally it should just about reach the actual desired setpoint but not quite before the temperature starts falling. Then by simply switching to the desired setpoint the integral  iTerm should be near the ideal spot for the Proportional control to take over and land setpoint. The integral stops changing because we are almost at the new desired setpoint also derivative influence will be at a minimum because the change in temperature has slowed.

Quote
What is the range of values that your system is producing and from which the error is calculated?
Balancing bot +- 25° angle input with an output of +-2550, My incubator 35°C ~ 40.5°C, input with an output of 0 ~ 950 PWN(timer2) My reflow oven 140°C ~ 220°C With a slowly ramping setpoint to a soak then a spike at the end to trigger the reflow process. output 0 ~ 2000 ms
My Tachometer has used RPM and microseconds to control the motor.  the output is simply 30-255 (0-30 has no influence on the motor.)

Quote
And I agree that my use of abs() is unnecessary - that's what comes from countless revisions and not seeing the trees for the wood.

...R
I saw the abs() there and thought there must be a way It can be simplified :)
Z
HC

Go Up