Diff: HbAttitude.cpp

Revision:

52:33fa8060dd8c

Parent:

48:71aec693a7dc

Child:

67:ad35921e3cdc

Child:

70:a7b1e3eec51b

diff -r 9a994998607b -r 33fa8060dd8c HbAttitude.cpp
--- a/HbAttitude.cpp	Tue Feb 26 09:50:10 2019 +0000
+++ b/HbAttitude.cpp	Mon Mar 04 07:55:48 2019 +0000
@@ -41,8 +41,8 @@
     float clcP= errAng * kp;
 
     //▼積分項
-    float delta_t = 1 / UPDATE_RATE; ////50HzとしているがUpdateRateのdefineからもってくるべき
-    float xi = ki * delta_t * (errAng + old) / 2;  //係数をかける
+    float delta_t = 1 / static_cast<float>(UPDATE_RATE); ////50HzとしているがUpdateRateのdefineからもってくるべき
+    float xi = ki * delta_t * (errAng + old) / 2.0;  //係数をかける
     sum = sum + xi;//積分して
 
     //リミット掛ける
@@ -50,7 +50,9 @@
     if(sum < limitL){sum = limitL;}
 
     //▼微分項
-    float clcD = kd * (iRate);
+    //float clcD = kd * (iRate);
+    //iRateの値が怪しいので位置から速度を計算する
+    float clcD = kd * (errAng - old) * UPDATE_RATE;
     //エラー量保存
     old =errAng;