Diff: headingHold.cpp

Revision:

0:2396598427bf

--- /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();
+}