Wataru Nakata / heading hold

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?

UserRevisionLine numberNew 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 }