Diff: bot/PIDcontroller/PID_controller.cpp

Revision:

22:682cc376da6f

Parent:

20:477c5d039e93

Child:

23:37bb9afe9fdc

--- a/bot/PIDcontroller/PID_controller.cpp	Tue Sep 05 14:07:18 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Thu Sep 07 06:26:53 2017 +0000
@@ -5,7 +5,17 @@
     confirm();
 }
 
-PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), JY901(HMCsda, HMCscl)
+PIDC::PIDC() :
+    PID(KC, TI, TD, INTERVAL),
+    JY901(HMCsda, HMCscl),
+    axisOffSetDegree(0),
+    planeOffSetDegree(0),
+    turnOverNumber(0),
+    beforeDegree(0),
+    rawDegree(0),
+    calculationResult(0),
+    axisCurrentDegree(0),
+    planeCurrentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -15,15 +25,23 @@
 
     //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
     rawDegree = JY901::getZaxisAngle();
-    beforeDegree = JY901::getZaxisAngle();
-    offSetDegree = JY901::getZaxisAngle();
-    initDegree = 0;
-    turnOverNumber = 0;
+    beforeDegree = rawDegree;
+    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), JY901(sda, scl)
+    PID(kc, ti, td, interval),
+    JY901(sda, scl),
+    axisOffSetDegree(0),
+    planeOffSetDegree(0),
+    turnOverNumber(0),
+    beforeDegree(0),
+    rawDegree(0),
+    calculationResult(0),
+    axisCurrentDegree(0),
+    planeCurrentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -32,27 +50,50 @@
     PID::setSetPoint(0.0);
 
     //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
-    rawDegree = JY901::getZaxisAngle();
-    beforeDegree = JY901::getZaxisAngle();
-    offSetDegree = JY901::getZaxisAngle();
-    initDegree = 0;
-    turnOverNumber = 0;
+    beforeDegree = rawDegree;
+    planeOffSetDegree = rawDegree;
+    axisOffSetDegree = rawDegree;
 //    this -> attach(this, &PIDC::updateOutput, INTERVAL);
 }
 
 
 void PIDC::confirm()
 {
-    rawDegree = getZaxisAngle();
-    if(rawDegree - beforeDegree < -1800) turnOverNumber++;
-    if(rawDegree - beforeDegree > 1800) turnOverNumber--;
-    initDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
+    rawDegree = JY901::getZaxisAngle();
+    if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
+    if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
+    axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 360;
+    planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 360;
     beforeDegree = JY901::getZaxisAngle();
-    PID::setProcessValue(initDegree / 10.0);
-    co = PID::compute();
+    PID::setProcessValue(axisCurrentDegree);
+    calculationResult = PID::compute();
+}
+
+float PIDC::getCalculationResult() const
+{
+    return calculationResult;
 }
 
-float PIDC::getCo()
+float PIDC::getCurrentDegree() const
+{
+    return planeCurrentDegree;
+}
+
+void PIDC::resetAxisOffset()
 {
-    return co;
+    beforeDegree = JY901::getZaxisAngle();
+    axisOffSetDegree = JY901::getZaxisAngle();
+    turnOverNumber = 0;
 }
+
+void PIDC::resetPlaneOffset()
+{
+    beforeDegree = JY901::getZaxisAngle();
+    planeOffSetDegree = JY901::getZaxisAngle();
+    turnOverNumber = 0;
+}
+
+void PIDC::calibration(int mode)
+{
+    //setCalibrationMode(mode);
+}