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:
- 54:c14c3bc48b8a
- Parent:
- 51:1e6334b993a3
- Child:
- 55:ee5257fb73df
File content as of revision 54:c14c3bc48b8a:
// sources used: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ #include "PID.h" #include "mbed.h" // PID is called with interrupt caller, so no need to keep track of time since dT is constant //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 (no need since called by interrupt) // 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 = *mySetpoint - 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::SetTunings(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(inAuto){ if(*myOutput > outMax) *myOutput = outMax; else if(*myOutput < outMin) *myOutput = outMin; if(errSum> outMax) errSum= outMax; else if(errSum< outMin) errSum= 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 = *myInput; errSum = *myOutput; lastErr = 0; if(errSum> outMax) errSum=outMax; else if(errSum<outMin) errSum=outMin; }