Robot's heading hold program using PID of input from sensor and output to motors
Diff: headingHold.cpp
- Revision:
- 0:2396598427bf
diff -r 000000000000 -r 2396598427bf headingHold.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/headingHold.cpp Thu Dec 07 00:54:11 2017 +0900 @@ -0,0 +1,57 @@ +#include "headingHold.h" + + +headingHold::headingHold(double p,double i,double d) : + headingPID(p,i,d,0.01) +{ + headingPID.setInputLimits(-180,180); + headingPID.setOutputLimits(-1,1); + headingPID.setBias(0.0); + headingPID.setMode(AUTO_MODE); + targetHeading = 0; + holding = false; +} + +bool headingHold::setHeadingHold(const bool& hold) +{ + if(holding == false) + { + targetHeading = currentHeading; + } + if(holding == hold) + return true; + holding = hold; + if(holding) + { + headingPID.reset(); + } + return 0; +} +bool headingHold::setCurrentHeading(const double& heading){ + if (fabs(heading) >180) + { + return 1; + } + currentHeading = heading; + return 0; +} + +bool headingHold::setTargetHeading(const double& target) +{ + if (fabs(target) >180) + { + return 1; + } + targetHeading = target; + return 0; +} + +double headingHold::getOutput() +{ + double error = targetHeading-currentHeading; + if(error < -180.0) error += 360; + else if(error > 180.0) error -= 360; + + headingPID.setProcessValue(error); + return headingPID.compute(); +}