Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
Revision 2:d1805e7d46fb, committed 2014-08-07
- Comitter:
- alpesh
- Date:
- Thu Aug 07 07:11:59 2014 +0000
- Parent:
- 1:a33723b70582
- Child:
- 3:e693c65b04de
- Commit message:
- Implementing PID Control;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Aug 05 13:09:58 2014 +0000 +++ b/main.cpp Thu Aug 07 07:11:59 2014 +0000 @@ -211,7 +211,7 @@ pc.putc(outChar + 48); } - pc.putc(','); + pc.putc('\t'); outChar = 0; @@ -269,7 +269,7 @@ pc.putc(outChar + 48); } - pc.putc(','); + pc.putc('\t'); outChar = 0; if (tempPow < 0.0) @@ -315,7 +315,7 @@ pc.putc(outChar + 48); } - pc.putc(','); + pc.putc('\t'); outChar = 0; if (tempSpeed < 0.0) @@ -391,7 +391,21 @@ RGHCosInterrupt.enable_irq(); } +double TargetSpeed=0; +double Error =0; +double KpTerm; +double ErrorDer; +double ErrorInt; + +double KpGain = 0.0; +double KiGain = 0.0; +double KdGain = 0.0; + +int counter; +double PreviousError [10]; + int main() + { RGHSinInterrupt.rise(&RGHSinHandler); RGHCosInterrupt.rise(&RGHCosHandler); @@ -404,10 +418,27 @@ Home(); - while(1) + //while(1) + + counter = 0; + + PreviousError[counter]=0; + + if (GetSpeed() <= TargetSpeed) + + { + Error = TargetSpeed - GetSpeedd(); + + KpTerm = Error * KpGain; + + ErrorDer = (Error - PreviousError[counter]) + } + { SerialTransmit(); + + switch(counter) { case 0: