Diff: bot/PIDcontroller/PID_controller.cpp

Revision:

6:590c9622ecf1

Child:

18:41f7dd1a5ed1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bot/PIDcontroller/PID_controller.cpp	Thu Aug 24 08:16:21 2017 +0000
@@ -0,0 +1,39 @@
+#include "PID_controller.h"
+
+void PIDC::updateOutput()
+{
+    confirm();
+}
+
+PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), HMC6352(HMCsda, HMCscl)
+{
+    setInputLimits(-180, 180);
+    setOutputLimits(-1.0, 1.0);
+    setBias(0.0);
+    setMode(AUTO_MODE);
+    setSetPoint(0.0);
+
+    setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    rawDegree = sample();
+    beforeDegree = sample();
+    offSetDegree = sample();
+    initDegree = 0;
+    turnOverNumber = 0;
+//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+}
+
+void PIDC::confirm()
+{
+    rawDegree = sample();
+    if(rawDegree - beforeDegree < -1800) turnOverNumber++;
+    if(rawDegree - beforeDegree > 1800) turnOverNumber--;
+    initDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
+    beforeDegree = sample(); 
+    setProcessValue(initDegree / 10.0);
+    co = compute();
+}
+
+float PIDC::getCo()
+{
+    return co;
+}
\ No newline at end of file