![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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:
- 54:c14c3bc48b8a
- Parent:
- 51:1e6334b993a3
- Child:
- 55:ee5257fb73df
--- a/PID/PID.cpp Fri Oct 09 11:44:05 2015 +0000 +++ b/PID/PID.cpp Fri Oct 09 12:08:35 2015 +0000 @@ -3,8 +3,9 @@ #include "PID.h" #include "mbed.h" -Timer t; // create Timer object t -t.start(); // start the timer +// 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){ @@ -18,20 +19,20 @@ SampleTime = 0.1; PID::SetTunings(Kp, Ki, Kd); - prevTime = t.read(); +// prevTime = t.read(); } -bool PID::compute(){ +bool PID::Compute(){ if(!inAuto) return false; - curTime = t.read() // read time in seconds - timeChange = curTime - prevTime; +// 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){ +// if(timeChange <=SampleTime){ float input = *myInput; - error = Setpoint - input; // get error for proportional + error = *mySetpoint - input; // get error for proportional errSum += error; // Error for integral // check for integrator windup if(errSum> outMax) errSum=outMax; @@ -46,13 +47,13 @@ else if(output<outMin) output=outMin; *myOutput = output; lastErr = error; - prevTime = curTime; +// prevTime = curTime; return true; - } - else return false; +// } +// else return false; } -void PID::SetTunigns(float Kp, float Ki, float Kd) +void PID::SetTunings(float Kp, float Ki, float Kd) { Kp = Kp; Ki = Ki * SampleTime; // change PID parameters according to sample time @@ -75,11 +76,13 @@ 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; + 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){ @@ -92,8 +95,9 @@ } void PID::Initialize(){ - lastInput = Input; - errSum = Output; + lastInput = *myInput; + errSum = *myOutput; + lastErr = 0; if(errSum> outMax) errSum=outMax; else if(errSum<outMin) errSum=outMin; } \ No newline at end of file