Diff: main.cpp

Revision:

10:21ea2a241025

Parent:

9:66b71a3f4ffb

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 {