Robot's heading hold program using PID of input from sensor and output to motors
headingHold.cpp@0:2396598427bf, 2017-12-07 (annotated)
- Committer:
- WAT34
- Date:
- Thu Dec 07 00:54:11 2017 +0900
- Revision:
- 0:2396598427bf
initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 0:2396598427bf | 1 | #include "headingHold.h" |
WAT34 | 0:2396598427bf | 2 | |
WAT34 | 0:2396598427bf | 3 | |
WAT34 | 0:2396598427bf | 4 | headingHold::headingHold(double p,double i,double d) : |
WAT34 | 0:2396598427bf | 5 | headingPID(p,i,d,0.01) |
WAT34 | 0:2396598427bf | 6 | { |
WAT34 | 0:2396598427bf | 7 | headingPID.setInputLimits(-180,180); |
WAT34 | 0:2396598427bf | 8 | headingPID.setOutputLimits(-1,1); |
WAT34 | 0:2396598427bf | 9 | headingPID.setBias(0.0); |
WAT34 | 0:2396598427bf | 10 | headingPID.setMode(AUTO_MODE); |
WAT34 | 0:2396598427bf | 11 | targetHeading = 0; |
WAT34 | 0:2396598427bf | 12 | holding = false; |
WAT34 | 0:2396598427bf | 13 | } |
WAT34 | 0:2396598427bf | 14 | |
WAT34 | 0:2396598427bf | 15 | bool headingHold::setHeadingHold(const bool& hold) |
WAT34 | 0:2396598427bf | 16 | { |
WAT34 | 0:2396598427bf | 17 | if(holding == false) |
WAT34 | 0:2396598427bf | 18 | { |
WAT34 | 0:2396598427bf | 19 | targetHeading = currentHeading; |
WAT34 | 0:2396598427bf | 20 | } |
WAT34 | 0:2396598427bf | 21 | if(holding == hold) |
WAT34 | 0:2396598427bf | 22 | return true; |
WAT34 | 0:2396598427bf | 23 | holding = hold; |
WAT34 | 0:2396598427bf | 24 | if(holding) |
WAT34 | 0:2396598427bf | 25 | { |
WAT34 | 0:2396598427bf | 26 | headingPID.reset(); |
WAT34 | 0:2396598427bf | 27 | } |
WAT34 | 0:2396598427bf | 28 | return 0; |
WAT34 | 0:2396598427bf | 29 | } |
WAT34 | 0:2396598427bf | 30 | bool headingHold::setCurrentHeading(const double& heading){ |
WAT34 | 0:2396598427bf | 31 | if (fabs(heading) >180) |
WAT34 | 0:2396598427bf | 32 | { |
WAT34 | 0:2396598427bf | 33 | return 1; |
WAT34 | 0:2396598427bf | 34 | } |
WAT34 | 0:2396598427bf | 35 | currentHeading = heading; |
WAT34 | 0:2396598427bf | 36 | return 0; |
WAT34 | 0:2396598427bf | 37 | } |
WAT34 | 0:2396598427bf | 38 | |
WAT34 | 0:2396598427bf | 39 | bool headingHold::setTargetHeading(const double& target) |
WAT34 | 0:2396598427bf | 40 | { |
WAT34 | 0:2396598427bf | 41 | if (fabs(target) >180) |
WAT34 | 0:2396598427bf | 42 | { |
WAT34 | 0:2396598427bf | 43 | return 1; |
WAT34 | 0:2396598427bf | 44 | } |
WAT34 | 0:2396598427bf | 45 | targetHeading = target; |
WAT34 | 0:2396598427bf | 46 | return 0; |
WAT34 | 0:2396598427bf | 47 | } |
WAT34 | 0:2396598427bf | 48 | |
WAT34 | 0:2396598427bf | 49 | double headingHold::getOutput() |
WAT34 | 0:2396598427bf | 50 | { |
WAT34 | 0:2396598427bf | 51 | double error = targetHeading-currentHeading; |
WAT34 | 0:2396598427bf | 52 | if(error < -180.0) error += 360; |
WAT34 | 0:2396598427bf | 53 | else if(error > 180.0) error -= 360; |
WAT34 | 0:2396598427bf | 54 | |
WAT34 | 0:2396598427bf | 55 | headingPID.setProcessValue(error); |
WAT34 | 0:2396598427bf | 56 | return headingPID.compute(); |
WAT34 | 0:2396598427bf | 57 | } |