control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
PID/PID.cpp
- Committer:
- annesteenbeek
- Date:
- 2015-10-09
- Revision:
- 51:1e6334b993a3
- Child:
- 54:c14c3bc48b8a
File content as of revision 51:1e6334b993a3:
// 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; }