Diff: bot/PIDcontroller/PID_controller.cpp

Revision:

5:16ea97725085

Parent:

3:369d9ee17e84

Child:

6:fe9767a50891

--- a/bot/PIDcontroller/PID_controller.cpp	Wed Sep 06 00:01:47 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Wed Sep 06 17:47:20 2017 +0900
@@ -22,10 +22,13 @@
     PID::setMode(AUTO_MODE);
     PID::setSetPoint(0.0);
 
+    wait(0.1);
     HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    wait(0.1);
     rawDegree = HMC6352::sample();
-    beforeDegree = HMC6352::sample();
-    offSetDegree = HMC6352::sample();
+    beforeDegree = rawDegree;
+    offSetDegree = rawDegree;
+    baseAngle = rawDegree;
     initDegree = 0;
     turnOverNumber = 0;
 //    this -> attach(this, &PIDC::updateOutput, INTERVAL);
@@ -48,12 +51,17 @@
     PID::setMode(AUTO_MODE);
     PID::setSetPoint(0.0);
 
+    wait(0.1);
     HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    wait(0.1);
     rawDegree = HMC6352::sample();
-    beforeDegree = HMC6352::sample();
-    offSetDegree = HMC6352::sample();
+    beforeDegree = rawDegree;
+    offSetDegree = rawDegree;
+    offSetDegree2 = rawDegree;
+    baseAngle = rawDegree;
     initDegree = 0;
     turnOverNumber = 0;
+
 //    this -> attach(this, &PIDC::updateOutput, INTERVAL);
 }
 
@@ -64,6 +72,7 @@
     if(rawDegree - beforeDegree < -1800) turnOverNumber++;
     if(rawDegree - beforeDegree > 1800) turnOverNumber--;
     initDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
+    initDegree2 = rawDegree - offSetDegree2 + turnOverNumber*3600;
     beforeDegree = HMC6352::sample();
     PID::setProcessValue(initDegree / 10.0);
     co = PID::compute();
@@ -74,11 +83,24 @@
     return co;
 }
 
-float PIDC::getDegree() const
+int PIDC::getDegree() const
 {
     return initDegree;
 }
 
+void PIDC::resetOffset()
+{
+    beforeDegree = HMC6352::sample();
+    offSetDegree = HMC6352::sample();
+    turnOverNumber = 0;
+}
+
+void PIDC::resetOffset2()
+{
+    beforeDegree = HMC6352::sample();
+    offSetDegree2 = HMC6352::sample();
+    turnOverNumber = 0;
+}
 
 void PIDC::calibration(int mode)
 {