control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
Diff: PID/PID.cpp
- Revision:
- 51:1e6334b993a3
- Child:
- 54:c14c3bc48b8a
diff -r b0cf07ca53cf -r 1e6334b993a3 PID/PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID/PID.cpp Fri Oct 09 13:32:29 2015 +0200 @@ -0,0 +1,99 @@ +// sources used: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ + +#include "PID.h" +#include "mbed.h" + +Timer t; // create Timer object t +t.start(); // start the timer + + +PID::PID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd){ + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + PID::SetOutputLimits(-1.0,1.0); // default output limits for FRDM board + + SampleTime = 0.1; + + PID::SetTunings(Kp, Ki, Kd); + prevTime = t.read(); +} + +bool PID::compute(){ + if(!inAuto) return false; + + curTime = t.read() // read time in seconds + timeChange = curTime - prevTime; + + // calculate using sample time frequency (regular intervals) + // no more need to modify derivative/int sicne sample time is constant + if(timeChange <=SampleTime){ + float input = *myInput; + error = Setpoint - input; // get error for proportional + errSum += error; // Error for integral + // check for integrator windup + if(errSum> outMax) errSum=outMax; + else if(errSum<outMin) errSum=outMin; + dErr = (error - lastErr); // get error for derivative + // (could still be tuned for derivative kick) + + // compute PID output + float output = Kp * error + Ki * errSum + Kd * dErr; + // make sure output is allso within min/max + if(output> outMax) output=outMax; + else if(output<outMin) output=outMin; + *myOutput = output; + lastErr = error; + prevTime = curTime; + return true; + } + else return false; +} + +void PID::SetTunigns(float Kp, float Ki, float Kd) +{ + Kp = Kp; + Ki = Ki * SampleTime; // change PID parameters according to sample time + Kd = Kd / SampleTime; +} + + +void PID::SetSampleTime(float NewSampleTime){ + if (NewSampleTime > 0 ){ + // change ratios for parameters for better computation in compute() + float ratio = NewSampleTime/SampleTime; + Ki *= ratio; + Kd /= ratio; + SampleTime = NewSampleTime; + } +} + +void PID::SetOutputLimits(float min, float max){ + if (min > max) return; + outMin = min; + outMax = max; + + if(Output > outMax) Output = outMax; + else if(Output < outMin) Output = outMin; + + if(ITerm> outMax) ITerm= outMax; + else if(ITerm< outMin) ITerm= outMin; +} + +void PID::SetMode(int Mode){ + // Turn PID on(AUTOMATIC) or off(MANUAL) + bool newAuto = (Mode == AUTOMATIC); + if(newAuto && !inAuto){ + Initialize(); // avoid bump because of stored values + } + inAuto = newAuto; +} + +void PID::Initialize(){ + lastInput = Input; + errSum = Output; + if(errSum> outMax) errSum=outMax; + else if(errSum<outMin) errSum=outMin; +} \ No newline at end of file