Diff: main.cpp

Revision:

9:66b71a3f4ffb

Parent:

8:2ae2483893f0

Child:

10:21ea2a241025

--- a/main.cpp	Tue Dec 01 06:10:15 2020 +0000
+++ b/main.cpp	Sun Dec 26 16:17:04 2021 +0000
@@ -29,10 +29,10 @@
 #define node 2
 
 // Disturbance Observer settings
-#define tfA 0.9981         
-#define tfB 0.7529          
-#define tfC 0.002457    
-#define tfD 0.000004588
+#define tfA 193.5
+#define tfB 0.7529
+#define tfC 1
+#define tfD 0.5219
 
 // 定数
 #define Kv 0.092421
@@ -72,6 +72,7 @@
 int cnt_run = 0;                           // 走行時間カウント
 bool run_eternal = false;
 int check = 0;
+bool decimal = false;
 
 //プロトタイプ宣言
 // CANopen関連
@@ -148,34 +149,98 @@
 void SerialRX(void){
     Serialdata = pc.getc();
     //-------------送信コマンドを選択--------------
+    if (decimal) {
+        if(Serialdata == '1'){
+            Velocity_Trend = 100;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '2'){
+            Velocity_Trend = 200;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '3'){
+            Velocity_Trend = 300;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '4'){
+            Velocity_Trend = 400;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '5'){
+            Velocity_Trend = 500;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '6'){
+            Velocity_Trend = 600;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '7'){
+            Velocity_Trend = 700;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '8'){
+            Velocity_Trend = 800;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '9'){
+            Velocity_Trend = 900;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        decimal = false;
+    } else {
+        if(Serialdata == '1'){
+            Velocity_Trend = 1000;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '2'){
+            Velocity_Trend = 2000;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '3'){
+            Velocity_Trend = 3000;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+        else if(Serialdata == '4'){
+            Velocity_Trend = 4000;
+            TgtVelCtrl(Velocity_Trend);
+            ENsig = SYNC_Enable;
+            Serialdata = 0;
+        }
+    }
+    
     if(Serialdata == 'q'){
         //Haltコマンド送信
         ENsig = SYNC_Disable;
         CtrlWord(QuickStop);
         Serialdata = 0;
     }
-    else if(Serialdata == '1'){
-        Velocity_Trend = -1000;
-        TgtVelCtrl(Velocity_Trend);
-        ENsig = SYNC_Enable;
-        Serialdata = 0;
-    }
-    else if(Serialdata == '2'){
-        Velocity_Trend = 2000;
-        TgtVelCtrl(Velocity_Trend);
-        ENsig = SYNC_Enable;
-        Serialdata = 0;
-    }
-    else if(Serialdata == '3'){
-        Velocity_Trend = 3000;
-        TgtVelCtrl(Velocity_Trend);
-        ENsig = SYNC_Enable;
-        Serialdata = 0;
-    }
-    else if(Serialdata == '4'){
-        Velocity_Trend = 4000;
-        TgtVelCtrl(Velocity_Trend);
-        ENsig = SYNC_Enable;
+    else if(Serialdata == '0'){
+        decimal = true;
+        ENsig = SYNC_Disable;
         Serialdata = 0;
     }
     else if(Serialdata == 'h'){
@@ -195,10 +260,10 @@
 void flip(void){
     if(canPort.read(canmsgRx)){
         cnt_run++;
-        if(canmsgRx.id == 0x4a0) printPlantData();
+        if(canmsgRx.id == 0x4a0) printPlantData();  // 左モータの情報を表示
     }
-    // 走行2分後に停止
-    if (cnt_run >= 12000 && run_eternal == false) {
+    // 走行4分後に停止
+    if (cnt_run >= 24000 && run_eternal == false) {
         //Haltコマンド送信
         ENsig = SYNC_Disable;
         CtrlWord(QuickStop);
@@ -217,12 +282,14 @@
     
     //Velocity_actual = canmsgRx.data[5]*0x100+canmsgRx.data[4];  //出力値の取得(速度)
     Velocity_raw = canmsgRx.data[5]*0x100+canmsgRx.data[4];
-    if (Velocity_raw > 10000) Velocity_actual = ((Velocity_raw -1) ^ 0xffff) * -1;
-    else Velocity_actual = Velocity_raw;
+    //if (Velocity_raw > 10000) Velocity_actual = ((Velocity_raw -1) ^ 0xffff) * -1;
+    //else Velocity_actual = Velocity_raw;
+    Velocity_actual = Velocity_raw;
     
     EstimateDisterbance();  //外乱の推定
     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) {
@@ -273,7 +340,6 @@
             cnt_ditect = 0;
         }
     }
-
 }
 //NMT
 void NMTPreOpn(void){