Diff: bot/PIDcontroller/PID_controller.cpp

Revision:

47:43f55ff8916b

Parent:

32:b619c7787dc3

Child:

51:794f160b09d4

--- a/bot/PIDcontroller/PID_controller.cpp	Sun Oct 29 15:40:26 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Mon Nov 06 18:27:03 2017 +0900
@@ -1,13 +1,9 @@
 #include "PID_controller.h"
 
-void PIDC::updateOutput()
-{
-    confirm();
-}
-
 PIDC::PIDC() :
-    PID(KC, TI, TD, INTERVAL),
-    HMC6352(HMCsda, HMCscl),
+    pid(KC, TI, TD, INTERVAL),
+    r1370(PC_6, PC_7),
+    pidcSerial(USBTX, USBRX, 115200),
     offSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
@@ -15,23 +11,26 @@
     calculationResult(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.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);
+    r1370.update();
     wait(0.1);
-    rawDegree = HMC6352::sample();
+    rawDegree = r1370.getRate();
     beforeDegree = rawDegree;
     offSetDegree = rawDegree;
+
+    pidcSerial.printf("pidc OK\r\n");
 }
 
-PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
-    PID(kc, ti, td, interval),
-    HMC6352(sda, scl),
+PIDC::PIDC(PinName tx, PinName rx, float kc, float ti, float td, float interval) :
+    pid(kc, ti, td, interval),
+    r1370(tx, rx),
+    pidcSerial(USBTX, USBRX, 115200),
     offSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
@@ -39,30 +38,30 @@
     calculationResult(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.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);
-    wait(0.1);
-    rawDegree = HMC6352::sample();
+    rawDegree = r1370.getRate();
     beforeDegree = rawDegree;
     offSetDegree = rawDegree;
+    pidcSerial.printf("pidc OK\r\n");
 }
 
 
 void PIDC::confirm()
 {
-    rawDegree = HMC6352::sample();
+    r1370.update();
+    rawDegree = r1370.getRate();
     if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
     if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
-    currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
-    beforeDegree = HMC6352::sample();
-    PID::setProcessValue(currentDegree / 10.0);
-    calculationResult = PID::compute();
+    currentDegree = rawDegree - offSetDegree + turnOverNumber * 360.0;
+    r1370.update();
+    beforeDegree = r1370.getRate();
+    pid.setProcessValue(currentDegree);
+    calculationResult = pid.compute();
 }
 
 float PIDC::getCalculationResult() const
@@ -70,29 +69,24 @@
     return calculationResult;
 }
 
-int PIDC::getCurrentDegree() const
+float PIDC::getCurrentDegree() const
 {
     return currentDegree;
 }
 
-int PIDC::getRawDegree()
+float PIDC::getRawDegree()
 {
-    return HMC6352::sample();
+    return r1370.getRate();
 }
 
 void PIDC::setPoint(float point)
 {
-    PID::setSetPoint(point);
+    pid.setSetPoint(point);
 }
 
 void PIDC::resetOffset()
 {
-    beforeDegree = HMC6352::sample();
-    offSetDegree = HMC6352::sample();
+    beforeDegree = r1370.getRate();
+    offSetDegree = r1370.getRate();
     turnOverNumber = 0;
 }
-
-void PIDC::calibration(int mode)
-{
-    setCalibrationMode(mode);
-}