Diff: bot/PIDcontroller/PID_controller.cpp

Branch:

develop1

Revision:

23:797d25f3df5e

Parent:

17:79fa65706f92

--- a/bot/PIDcontroller/PID_controller.cpp	Thu Sep 28 20:37:09 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Sun Oct 01 11:43:53 2017 +0900
@@ -1,77 +1,67 @@
 #include "PID_controller.h"
 
-void PIDC::updateOutput()
-{
-    confirm();
-}
-
 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);
-    PID::setBias(BIAS);
-    PID::setMode(AUTO_MODE);
-    PID::setSetPoint(0.0);
+    pid = new PID(KC, TI, TD, INTERVAL);
+    pid -> setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
+    pid -> setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
+    pid -> setBias(BIAS);
+    pid -> setMode(AUTO_MODE);
+    pid -> setSetPoint(0.0);
 
     wait(0.1);
-    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    hmc -> setOpMode(HMC6352_CONTINUOUS, 1, 20);
     wait(0.1);
-    rawDegree = HMC6352::sample();
+    rawDegree = hmc -> 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);
-    PID::setBias(BIAS);
-    PID::setMode(AUTO_MODE);
-    PID::setSetPoint(0.0);
+    pid = new PID(kc, ti, td, interval);
+    hmc = new HMC6352(sda, scl);
+    pid -> setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
+    pid -> setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
+    pid -> setBias(BIAS);
+    pid -> setMode(AUTO_MODE);
+    pid -> setSetPoint(0.0);
 
     wait(0.1);
-    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    hmc -> setOpMode(HMC6352_CONTINUOUS, 1, 20);
     wait(0.1);
-    rawDegree = HMC6352::sample();
+    rawDegree = hmc -> sample();
     beforeDegree = rawDegree;
-    planeOffSetDegree = rawDegree;
-    axisOffSetDegree = rawDegree;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+    offSetDegree = rawDegree;
 }
 
 
 void PIDC::confirm()
 {
-    rawDegree = HMC6352::sample();
+    rawDegree = hmc -> sample();
     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(axisCurrentDegree / 10.0);
-    calculationResult = PID::compute();
+    currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
+    beforeDegree = hmc -> sample();
+    pid -> setProcessValue(currentDegree / 10.0);
+    calculationResult = pid -> compute();
+}
+
+void PIDC::setPoint(float point)
+{
+    pid -> setSetPoint(point);
 }
 
 float PIDC::getCalculationResult() const
@@ -81,29 +71,22 @@
 
 int PIDC::getCurrentDegree() const
 {
-    return planeCurrentDegree;
+    return currentDegree;
 }
 
 int PIDC::getRawDegree()
 {
-    return HMC6352::sample();
+    return hmc -> sample();
 }
 
-void PIDC::resetAxisOffset()
+void PIDC::resetOffset()
 {
-    beforeDegree = HMC6352::sample();
-    axisOffSetDegree = HMC6352::sample();
-    turnOverNumber = 0;
-}
-
-void PIDC::resetPlaneOffset()
-{
-    beforeDegree = HMC6352::sample();
-    planeOffSetDegree = HMC6352::sample();
+    beforeDegree = hmc -> sample();
+    offSetDegree = hmc -> sample();
     turnOverNumber = 0;
 }
 
 void PIDC::calibration(int mode)
 {
-    setCalibrationMode(mode);
+    hmc -> setCalibrationMode(mode);
 }