disturbance observer
Dependencies: mbed MATSUbed USBDevice
Revision 10:21ea2a241025, committed 2021-12-27
- Comitter:
- Mtshadow
- Date:
- Mon Dec 27 06:56:54 2021 +0000
- Parent:
- 9:66b71a3f4ffb
- Commit message:
- success detect nonmagnet at horizon
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 66b71a3f4ffb -r 21ea2a241025 main.cpp --- a/main.cpp Sun Dec 26 16:17:04 2021 +0000 +++ b/main.cpp Mon Dec 27 06:56:54 2021 +0000 @@ -38,8 +38,8 @@ #define Kv 0.092421 #define Km 0.0092 #define samplimgrate 0.01 -#define bodylength 550.0 -#define startdetect 10.0 +#define startdetect 20.0 +#define enddetect 200.0 #define Kn 116 USBSerial pc; //PCとのシリアル通信 @@ -63,17 +63,19 @@ int Velocity_Trend=0, Current_Trend=0; //除去するトレンド float distance = 0; //走行距離[mm] float Velocity_meter; // 走行速度[mm/s] -float Distterbance_estimate_tf_abs = 0; // 外乱推定値nの絶対値 -float Dist_error = 0; -float baseDist = 0; // 判別基準値 -int cnt_ditect = 0; // しきい値を超えてからのカウント -int cnt_find = 0; // 非磁性面の検出からのカウント -int findnonmagnet =0; // 非磁性面検出信号 -int cnt_run = 0; // 走行時間カウント bool run_eternal = false; int check = 0; bool decimal = false; +// 判別用 +float Distterbance_estimate_tf_abs = 0; // 外乱推定値nの絶対値 +float Dist_error = 0; // 低下割合 +float baseDist = 0; // 判別基準値 +int cnt_ditect = 0; // しきい値を超えてからのカウント +int cnt_find = 0; // 非磁性面の検出からのカウント +int findnonmagnet =0; // 非磁性面検出信号 +int cnt_run = 0; // 走行時間カウント + //プロトタイプ宣言 // CANopen関連 void NMTPreOpn(void); //NMT PreOperationalへ移行 @@ -290,6 +292,7 @@ detectNonmagnet(); //非磁性面の判別 pc.printf("%f,%f,%f,%f,%f,%f,%d\r\n",Current_demand,Velocity_actual,Torque_estimate_tf,Distterbance_estimate_tf,baseDist,Dist_error,findnonmagnet); + // ロボット停止処理 if ( findnonmagnet == 1 && run_eternal == false) { /*cnt_find++; if (cnt_find >= 200) { @@ -297,8 +300,8 @@ ENsig = SYNC_Disable; CtrlWord(QuickStop); }*/ - //ENsig = SYNC_Disable; - //CtrlWord(QuickStop); + ENsig = SYNC_Disable; + CtrlWord(QuickStop); } findnonmagnet = 0; } @@ -324,16 +327,20 @@ Velocity_meter = Velocity_actual * Kv; //回転数から速度に変換[m/s] distance += Velocity_meter * samplimgrate; //走行距離を計算 Distterbance_estimate_tf_abs = fabsf(Distterbance_estimate_tf); - if (distance <= bodylength && distance >= startdetect) { + if (distance <= enddetect && distance >= startdetect) { check = 0; //外乱推定値の最大値を確認 if(Distterbance_estimate_tf_abs > baseDist) baseDist = Distterbance_estimate_tf_abs; - } else if (distance > bodylength) { + } else if (distance > enddetect) { + // 低下割合算出 Dist_error = Distterbance_estimate_tf_abs / baseDist; - if (Dist_error < 0.3) { + + // しきい値判断 + if (Dist_error < 0.5) { check = 1; cnt_ditect++; - if(cnt_ditect >= 90) { + // 低下時間判断 + if(cnt_ditect >= 50) { findnonmagnet = 1; } } else {