Diff: bot/PIDcontroller/PID_controller.cpp

Revision:

6:fe9767a50891

Parent:

5:16ea97725085

Child:

17:79fa65706f92

--- a/bot/PIDcontroller/PID_controller.cpp	Wed Sep 06 17:47:20 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Wed Sep 06 18:39:19 2017 +0900
@@ -8,13 +8,14 @@
 PIDC::PIDC() :
     PID(KC, TI, TD, INTERVAL),
     HMC6352(HMCsda, HMCscl),
-    rawDegree(0),
-    offSetDegree(0),
+    axisOffSetDegree(0),
+    planeOffSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
-    co(0),
-    processValue(0),
-    initDegree(0)
+    rawDegree(0),
+    calculationResult(0),
+    axisCurrentDegree(0),
+    planeCurrentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -27,23 +28,22 @@
     wait(0.1);
     rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    offSetDegree = rawDegree;
-    baseAngle = rawDegree;
-    initDegree = 0;
-    turnOverNumber = 0;
+    planeOffSetDegree = rawDegree;
+    axisOffSetDegree = rawDegree;
 //    this -> attach(this, &PIDC::updateOutput, INTERVAL);
 }
 
 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
     PID(kc, ti, td, interval),
     HMC6352(sda, scl),
-    rawDegree(0),
-    offSetDegree(0),
+    axisOffSetDegree(0),
+    planeOffSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
-    co(0),
-    processValue(0),
-    initDegree(0)
+    rawDegree(0),
+    calculationResult(0),
+    axisCurrentDegree(0),
+    planeCurrentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -56,12 +56,8 @@
     wait(0.1);
     rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    offSetDegree = rawDegree;
-    offSetDegree2 = rawDegree;
-    baseAngle = rawDegree;
-    initDegree = 0;
-    turnOverNumber = 0;
-
+    planeOffSetDegree = rawDegree;
+    axisOffSetDegree = rawDegree;
 //    this -> attach(this, &PIDC::updateOutput, INTERVAL);
 }
 
@@ -69,36 +65,36 @@
 void PIDC::confirm()
 {
     rawDegree = HMC6352::sample();
-    if(rawDegree - beforeDegree < -1800) turnOverNumber++;
-    if(rawDegree - beforeDegree > 1800) turnOverNumber--;
-    initDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
-    initDegree2 = rawDegree - offSetDegree2 + turnOverNumber*3600;
+    if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
+    if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
+    axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 3600;
+    planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 3600;
     beforeDegree = HMC6352::sample();
-    PID::setProcessValue(initDegree / 10.0);
-    co = PID::compute();
+    PID::setProcessValue(axisCurrentDegree / 10.0);
+    calculationResult = PID::compute();
 }
 
-float PIDC::getCo() const
+float PIDC::getCalculationResult() const
 {
-    return co;
+    return calculationResult;
 }
 
-int PIDC::getDegree() const
+int PIDC::getCurrentDegree() const
 {
-    return initDegree;
+    return planeCurrentDegree;
 }
 
-void PIDC::resetOffset()
+void PIDC::resetAxisOffset()
 {
     beforeDegree = HMC6352::sample();
-    offSetDegree = HMC6352::sample();
+    axisOffSetDegree = HMC6352::sample();
     turnOverNumber = 0;
 }
 
-void PIDC::resetOffset2()
+void PIDC::resetPlaneOffset()
 {
     beforeDegree = HMC6352::sample();
-    offSetDegree2 = HMC6352::sample();
+    planeOffSetDegree = HMC6352::sample();
     turnOverNumber = 0;
 }