Diff: main.cpp

Revision:

7:df29c4de6522

Parent:

6:07534de7cdb4

Child:

8:c9a5ef6f003f

--- a/main.cpp	Sun Dec 19 17:23:45 2021 +0000
+++ b/main.cpp	Sun Dec 19 18:48:16 2021 +0000
@@ -18,6 +18,7 @@
 
 #define KICK 2000
 
+#define WALL 45
 
  
 DigitalOut led1(LED1);
@@ -75,7 +76,19 @@
 void vel_stop(void);
 void vel_forward(int);
 void vel_backward(int);
+void vel_right(int);
+void vel_left(int);
 
+//unsigned int get_cm_n(HCSR04, unsigned int);
+//USE -> unsigned int dist_UnitA = get_cm_n(u2, 5);
+unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){
+    unsigned int sampled_dist=0;
+    for (int iter_n = 0; iter_n <echo_n; iter_n++){
+        echo_unit.start();
+        sampled_dist += echo_unit.get_dist_cm();
+    }
+    return  (sampled_dist / echo_n);
+} 
 
 int nodeall=4;
  
@@ -93,7 +106,7 @@
     int node4 = 4;
     
     //モータ回転数
-    int rpm = 600;
+    int rpm = 400;
     
     //エンコーダ関係
     int ActPos = 0;
@@ -156,68 +169,112 @@
              
     int state_1 = 0;
 
-    while(1){  
-                      
-        ActPos = 0;
-        Init_Pos = 0;
-        dist1 = 0;
+    ActPos = 0;
+    Init_Pos = 0;
+    dist1 = 0;
+    readActPos(1);
+    ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
+    if(ActPos > 8388608){
+        ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
+    }
+    Init_Pos = ActPos;//起動時の角度を保存
+    t.reset();
+    t.start();
+
+    //set_MODE_T();
+
+    printf("\nstart\r\n");
+
+    while(1){
+                
+        Time = t.read();
+                
         readActPos(1);
         ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
         if(ActPos > 8388608){
             ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
+            //printf("check\r\n");
         }
-        Init_Pos = ActPos;//起動時の角度を保存
-        t.reset();
-        t.start();
-
-        set_MODE_T();
- 
-        while(1){
-                 
-            Time = t.read();
-                 
-            readActPos(1);
-            ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
-            if(ActPos > 8388608){
-                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();    
+        //u1.start();
+        //u2.start();
                         
-            //printf("dsit1 %d\r\n",dist1);
-            printf("%d\r\n",state_1);
-            /*--------------------------*/
-            //前方向に入力があった時
-            if(state_1 == 0){//入力判断フェーズ
-                if(ActPos < Init_Pos - KICK){
-                    state_1 = 1;
-                }else{
+        //dist1 = u1.get_dist_cm();
+        //dist2 = u2.get_dist_cm();    
+
+        dist1 = get_cm_n(u1, 5);
+        dist2 = get_cm_n(u2, 5);
+        dist3 = get_cm_n(u3, 5);
                     
-                }
-            }else if(state_1 == 1){//前進→壁検出フェーズ
-                if(dist1 < 50 && dist1 >= 35){
-                    state_1 = 2;
-                }else{
-                    set_ACC(ACC_B);//加速度設定
-                    set_DEC(ACC_C);//減速度設定
-                    set_MODE_V();//速度制御モード送信
-                    vel_forward(rpm);//前進速度指令
-                }
-            }else if(state_1 == 2){//帰還フェーズ
-                if(ActPos > -10000){
-                    vel_stop();
-                    state_1 = 0;
-                    wait(3.0);
-                    break;
-                }else{
-                    vel_backward(rpm);
-                }
-            }     
-        }      
+        //printf("dsit1 %d\r\n",dist1);
+        printf("%d\r\n",state_1);
+        /*--------------------------*/
+        //前方向に入力があった時
+        if(state_1 == 0){//入力判断フェーズ
+            if(ActPos < (Init_Pos - KICK)){
+                state_1 = 1;
+            }else if(ActPos > (Init_Pos + KICK)){
+                state_1 = 11;
+            }else{
+                set_MODE_T();
+            }
+        }else if(state_1 == 1){//前進→壁検出フェーズ
+            if(dist1 < WALL && dist1 >= 25){
+                state_1 = 2;
+            }else{
+                set_ACC(ACC_B);//加速度設定
+                set_DEC(ACC_C);//減速度設定
+                set_MODE_V();//速度制御モード送信
+                vel_forward(rpm);//前進速度指令
+            }
+        }else if(state_1 == 2){//前進からの帰還フェーズ
+            if(ActPos > -3000){
+                vel_stop();
+                state_1 = 0;
+                wait(1.0);
+                //break;
+            }else{
+                vel_backward(rpm);
+            }
+        }else if(state_1 == 11){//右進→壁検出フェーズ
+            if(dist3 < WALL && dist3 >= 25){
+                state_1 = 12;
+            }else{
+                set_ACC(ACC_B);//加速度設定
+                set_DEC(ACC_C);//減速度設定
+                set_MODE_V();//速度制御モード送信
+                vel_right(rpm);//前進速度指令
+            }
+        }else if(state_1 == 12){//右進からの帰還フェーズ
+            if(ActPos < 3000){
+                vel_stop();
+                state_1 = 0;
+                wait(1.0);
+                //break;
+            }else{
+                vel_left(rpm);
+            }
+        }
+    }      
+        
+}
+
+void vel_right(int rpm){
+    sendTgtVel(1,rpm);
+    sendTgtVel(2,rpm*(-1));
+    sendTgtVel(3,rpm*(-1));
+    sendTgtVel(4,rpm);
+    for(int i=1;i<= 4;i++){
+        sendCtrlEN(i);
+    }
+}
+
+void vel_left(int rpm){
+    sendTgtVel(1,rpm*(-1));
+    sendTgtVel(2,rpm);
+    sendTgtVel(3,rpm);
+    sendTgtVel(4,rpm*(-1));
+    for(int i=1;i<= 4;i++){
+        sendCtrlEN(i);
     }
 }