Diff: bot/PIDcontroller/PID_controller.cpp

Branch:

develop

Revision:

32:b619c7787dc3

Parent:

17:79fa65706f92

Child:

47:43f55ff8916b

--- a/bot/PIDcontroller/PID_controller.cpp	Thu Oct 05 20:09:43 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Fri Oct 06 20:12:29 2017 +0900
@@ -8,14 +8,12 @@
 PIDC::PIDC() :
     PID(KC, TI, TD, INTERVAL),
     HMC6352(HMCsda, HMCscl),
-    axisOffSetDegree(0),
-    planeOffSetDegree(0),
+    offSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
     rawDegree(0),
     calculationResult(0),
-    axisCurrentDegree(0),
-    planeCurrentDegree(0)
+    currentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -28,22 +26,18 @@
     wait(0.1);
     rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    planeOffSetDegree = rawDegree;
-    axisOffSetDegree = rawDegree;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+    offSetDegree = rawDegree;
 }
 
 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
     PID(kc, ti, td, interval),
     HMC6352(sda, scl),
-    axisOffSetDegree(0),
-    planeOffSetDegree(0),
+    offSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
     rawDegree(0),
     calculationResult(0),
-    axisCurrentDegree(0),
-    planeCurrentDegree(0)
+    currentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -56,9 +50,7 @@
     wait(0.1);
     rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    planeOffSetDegree = rawDegree;
-    axisOffSetDegree = rawDegree;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+    offSetDegree = rawDegree;
 }
 
 
@@ -67,10 +59,9 @@
     rawDegree = HMC6352::sample();
     if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
     if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
-    axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 3600;
-    planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 3600;
+    currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
     beforeDegree = HMC6352::sample();
-    PID::setProcessValue(axisCurrentDegree / 10.0);
+    PID::setProcessValue(currentDegree / 10.0);
     calculationResult = PID::compute();
 }
 
@@ -81,7 +72,7 @@
 
 int PIDC::getCurrentDegree() const
 {
-    return planeCurrentDegree;
+    return currentDegree;
 }
 
 int PIDC::getRawDegree()
@@ -89,17 +80,15 @@
     return HMC6352::sample();
 }
 
-void PIDC::resetAxisOffset()
+void PIDC::setPoint(float point)
+{
+    PID::setSetPoint(point);
+}
+
+void PIDC::resetOffset()
 {
     beforeDegree = HMC6352::sample();
-    axisOffSetDegree = HMC6352::sample();
-    turnOverNumber = 0;
-}
-
-void PIDC::resetPlaneOffset()
-{
-    beforeDegree = HMC6352::sample();
-    planeOffSetDegree = HMC6352::sample();
+    offSetDegree = HMC6352::sample();
     turnOverNumber = 0;
 }