Diff: bot/PIDcontroller/PID_controller.cpp

Revision:

26:7258d5ad0bff

Parent:

23:37bb9afe9fdc

--- a/bot/PIDcontroller/PID_controller.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -7,15 +7,13 @@
 
 PIDC::PIDC() :
     PID(KC, TI, TD, INTERVAL),
-    JY901(HMCsda, HMCscl),
-    axisOffSetDegree(0),
-    planeOffSetDegree(0),
+    HMC6352(HMCsda, HMCscl),
+    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);
@@ -23,24 +21,23 @@
     PID::setMode(AUTO_MODE);
     PID::setSetPoint(0.0);
 
-    rawDegree = JY901::getZaxisAngle();
+    wait(0.1);
+    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    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),
-    JY901(sda, scl),
-    axisOffSetDegree(0),
-    planeOffSetDegree(0),
+    HMC6352(sda, scl),
+    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);
@@ -48,23 +45,23 @@
     PID::setMode(AUTO_MODE);
     PID::setSetPoint(0.0);
 
-    rawDegree = JY901::getZaxisAngle();
+    wait(0.1);
+    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    wait(0.1);
+    rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    planeOffSetDegree = rawDegree;
-    axisOffSetDegree = rawDegree;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+    offSetDegree = rawDegree;
 }
 
 
 void PIDC::confirm()
 {
-    rawDegree = JY901::getZaxisAngle();
+    rawDegree = HMC6352::sample();
     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(axisCurrentDegree);
+    currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
+    beforeDegree = HMC6352::sample();
+    PID::setProcessValue(currentDegree / 10.0);
     calculationResult = PID::compute();
 }
 
@@ -73,31 +70,29 @@
     return calculationResult;
 }
 
-float PIDC::getCurrentDegree() const
+int PIDC::getCurrentDegree() const
 {
-    return planeCurrentDegree;
+    return currentDegree;
+}
+
+int PIDC::getRawDegree()
+{
+    return HMC6352::sample();
 }
 
-void PIDC::resetAxisOffset()
+void PIDC::setPoint(float point)
 {
-    beforeDegree = JY901::getZaxisAngle();
-    axisOffSetDegree = JY901::getZaxisAngle();
-    turnOverNumber = 0;
+    PID::setSetPoint(point);
 }
 
-void PIDC::resetPlaneOffset()
+void PIDC::resetOffset()
 {
-    beforeDegree = JY901::getZaxisAngle();
-    planeOffSetDegree = JY901::getZaxisAngle();
+    beforeDegree = HMC6352::sample();
+    offSetDegree = HMC6352::sample();
     turnOverNumber = 0;
 }
 
-void PIDC::renewAngle()
-{
-  angle = JY901::getZaxisAngle();
-}
-
 void PIDC::calibration(int mode)
 {
-
+    setCalibrationMode(mode);
 }