teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

Diff: HbAttitude.cpp

Revision:
52:33fa8060dd8c
Parent:
48:71aec693a7dc
Child:
67:ad35921e3cdc
Child:
70:a7b1e3eec51b
--- 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;