Diff: main.cpp

Revision:

4:e1aa0b348aeb

Parent:

3:c39c14cfc811

Child:

5:a7c3f446a1f1

--- a/main.cpp	Sun Dec 19 11:16:07 2021 +0000
+++ b/main.cpp	Sun Dec 19 15:43:05 2021 +0000
@@ -167,6 +167,7 @@
                       
         ActPos = 0;
         Init_Pos = 0;
+        dist1 = 0;
         readActPos(1);
         ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
         if(ActPos > 8388608){
@@ -186,10 +187,16 @@
                     ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
                    //printf("check\r\n");
                 }
+                 u1.start();
+                 u2.start();
+                            
+                        dist1 = u1.get_dist_cm();
+                        dist2 = u2.get_dist_cm();    
+                        
                 printf("dsit1 %d\r\n",dist1);
                 /*--------------------------*/
                 //前方向に入力があった時
-                if(ActPos < Init_Pos - KICK){
+                if(0){//ActPos < Init_Pos - KICK){
                                   
                     set_ACC(ACC_B);
                     set_DEC(ACC_C);
@@ -283,122 +290,9 @@
                  
                  /*--------------------------*/
                  //右方向に入力があった時
-                if(ActPos > Init_Pos + 2000){
-                    set_ACC(ACC_B);
-                    set_DEC(ACC_C);
                 
-                    //速度制御モード送信    
-                    set_MODE_V();
-
-                    //速度を指定
-                    sendTgtVel(1,rpm);
-                    sendTgtVel(2,rpm*(-1));
-                    sendTgtVel(3,rpm*(-1));
-                    sendTgtVel(4,rpm);
-                    //指令値を送信
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);
-                    }
-                    //実行時間
-                    wait(2.0);
-                 
-                    //速度を指定
-                    sendTgtVel(1,0);
-                    sendTgtVel(2,0);
-                    sendTgtVel(3,0);
-                    sendTgtVel(4,0);
-                    //指令値を送信
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);    
-                    }
-                    //実行時間
-                    wait(5.0);
- 
-                 
-                    //速度を指定
-                    sendTgtVel(1,rpm*(-1));
-                    sendTgtVel(2,rpm);
-                    sendTgtVel(3,rpm);
-                    sendTgtVel(4,rpm*(-1));
-                    //指令値を送信
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);    
-                    }
-                    //実行時間
-                    wait(2.0);
- 
-                    //ブレーキ
-                    sendTgtVel(1,0);
-                    sendTgtVel(2,0);
-                    sendTgtVel(3,0);
-                    sendTgtVel(4,0);
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);    
-                    }
-                    wait(0.5);
-                 
-                    set_MODE_T();
-                 
-                    wait(4.0);
-                    break;
-                }
-                //一定時間入力がない場合
-                if(Time > Standby_Time){
-                    sendProAcc(1,5000);
-                    sendProAcc(2,5000);
-                    sendProAcc(3,5000);
-                    sendProAcc(4,5000);
-           
-                    sendProDec(1,5000);
-                    sendProDec(2,5000);
-                    sendProDec(3,5000);
-                    sendProDec(4,5000);  
-        
-                    //速度制御モード送信    
-                    set_MODE_V();
-
-                    //速度を指定
-                    sendTgtVel(1,rpm);
-                    sendTgtVel(2,rpm);
-                    sendTgtVel(3,rpm);
-                    sendTgtVel(4,rpm);
-                    //指令値を送信
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);
-                    }
-                    //実行時間
-                     wait(1.0);
-                    
-                    //速度を指定
-                    sendTgtVel(1,rpm*(-1));
-                    sendTgtVel(2,rpm*(-1));
-                    sendTgtVel(3,rpm*(-1));
-                    sendTgtVel(4,rpm*(-1));
-                    //指令値を送信
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);    
-                    }
-                    //実行時間
-                    wait(1.3);
-    
-                    //ブレーキ
-                     sendTgtVel(1,0);
-                     sendTgtVel(2,0);
-                    sendTgtVel(3,0);
-                    sendTgtVel(4,0);
-                    for(int i=1;i<= 4;i++){
-                        sendCtrlEN(i);    
-                    }
-    
-                    wait(0.5);
-                    
-                    set_MODE_T();
-                    
-                    wait(5.0);
-                    break;
-                }         
            }     
-                 wait(0.1);   
+                 wait(0.5);   
         }
 }