Revision 0:2396598427bf, committed 2017-12-07

Comitter:

WAT34

Date:

Thu Dec 07 00:54:11 2017 +0900

Commit message:

initial commit

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/headingHold.h	Thu Dec 07 00:54:11 2017 +0900
@@ -0,0 +1,23 @@
+#ifndef HEADING_HOLD_H
+#define HEADING_HOLD_H
+#include "mbed.h"
+#include "PID.h"
+
+class headingHold {
+private:
+  bool holding;
+  float currentHeading;
+  float targetHeading;
+  PID headingPID;
+
+public:
+  headingHold (double p,double i,double d);
+  bool setHeadingHold(const bool& hold);
+  bool setCurrentHeading(const double& heading);
+  bool setTargetHeading(const double& target);
+  double getOutput();
+
+};
+
+
+#endif /* end of include guard: HEADING_HOLD_H */