Diff: main.cpp

Revision:

9:51325cc6496a

Parent:

8:0497ad6e03a4

--- a/main.cpp	Sun Dec 19 20:24:17 2021 +0000
+++ b/main.cpp	Mon Dec 20 05:53:58 2021 +0000
@@ -74,9 +74,13 @@
 void set_MODE_T(void);
 
 void vel_stop(void);
-void vel_forward(int,int,int);
+
+void vel_forward(int);
+void vel_forward_con(int);
 void vel_backward(int);
+void vel_backward_con(int);
 void vel_right(int);
+void vel_right_con(int);
 void vel_left(int);
 
 //unsigned int get_cm_n(HCSR04, unsigned int);
@@ -195,20 +199,13 @@
             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();    
-
         dist1 = get_cm_n(u1, 5);
         dist2 = get_cm_n(u2, 5);
         dist3 = get_cm_n(u3, 5);
-                    
-        //printf("dsit1 %d\r\n",dist1);
+         
         printf("%d\r\n",state_1);
         /*--------------------------*/
-        //前方向に入力があった時
+        //
         if(state_1 == 0){//入力判断フェーズ
             if(ActPos < (Init_Pos - KICK)){
                 state_1 = 1;
@@ -217,6 +214,7 @@
             }else{
                 set_MODE_T();
             }
+            
         }else if(state_1 == 1){//前進→壁検出フェーズ
             if(dist1 < WALL && dist1 >= 25){
                 state_1 = 2;
@@ -224,10 +222,7 @@
                 set_ACC(ACC_B);//加速度設定
                 set_DEC(ACC_C);//減速度設定
                 set_MODE_V();//速度制御モード送信
-               dist1 = get_cm_n(u1, 5);
-                dist2 = get_cm_n(u2, 5);
-
-                vel_forward(rpm,dist1,dist2);//前進速度指令
+                vel_forward_con(rpm);//前進速度指令
             }
         }else if(state_1 == 2){//前進からの帰還フェーズ
             if(ActPos > -3000){
@@ -236,7 +231,7 @@
                 wait(1.0);
                 //break;
             }else{
-                vel_backward(rpm);
+                vel_backward_con(rpm);
             }
         }else if(state_1 == 11){//右進→壁検出フェーズ
             if(dist3 < WALL && dist3 >= 25){
@@ -245,7 +240,7 @@
                 set_ACC(ACC_B);//加速度設定
                 set_DEC(ACC_C);//減速度設定
                 set_MODE_V();//速度制御モード送信
-                vel_right(rpm);//前進速度指令
+                vel_right_con(rpm);//R進速度指令
             }
         }else if(state_1 == 12){//右進からの帰還フェーズ
             if(ActPos < 3000){
@@ -255,11 +250,21 @@
                 //break;
             }else{
                 vel_left(rpm);
-            }
+            } 
+        }else if(state_1 == 41){
+            //Zig Zag
+            if(dist1 < WALL && dist1 >= 25){
+                state_1 = 2;
+            }else{
+                set_ACC(ACC_B);//加速度設定
+                set_DEC(ACC_C);//減速度設定
+                set_MODE_V();//速度制御モード送信
+                vel_forward_con(rpm);//前進速度指令
         }
     }      
         
 }
+}
 
 void vel_right(int rpm){
     sendTgtVel(1,rpm);
@@ -270,6 +275,22 @@
         sendCtrlEN(i);
     }
 }
+void vel_right_con(int rpmA){
+    int dis1 = get_cm_n(u1,15);
+    int dis2 = get_cm_n(u2,15);
+    
+    //速度を指定
+    int robot_angle = ((dis1 - dis2)*5);
+    sendTgtVel(1,rpmA+robot_angle);
+    sendTgtVel(2,rpmA*(-1)+robot_angle);
+    sendTgtVel(3,rpmA*(-1)+robot_angle);
+    sendTgtVel(4,rpmA+robot_angle);
+    //指令値を送信
+    for(int i=1;i<= 4;i++){
+        sendCtrlEN(i);
+    }
+}
+ 
 
 void vel_left(int rpm){
     sendTgtVel(1,rpm*(-1));
@@ -281,6 +302,23 @@
     }
 }
 
+
+void vel_left_con(int rpmA){
+    int dis1 = get_cm_n(u1,15);
+    int dis2 = get_cm_n(u2,15);
+    
+    //速度を指定
+    int robot_angle = ((dis1 - dis2)*5);
+    sendTgtVel(1,rpmA*(-1)+robot_angle);
+    sendTgtVel(2,rpmA+robot_angle);
+    sendTgtVel(3,rpmA+robot_angle);
+    sendTgtVel(4,rpmA*(-1)+robot_angle);
+    //指令値を送信
+    for(int i=1;i<= 4;i++){
+        sendCtrlEN(i);
+    }
+}
+
 void vel_stop(){
     //速度を指定
     sendTgtVel(1,0);
@@ -305,9 +343,29 @@
     }
 }
 
-void vel_forward(int rpmA,int dis1,int dis2){
+void vel_backward_con(int rpmA){
+    int dis1 = get_cm_n(u1,15);
+    int dis2 = get_cm_n(u2,15);
+    
     //速度を指定
-    int robot_angle = ((dis1 - dis2)*50);
+    int robot_angle = ((dis1 - dis2)*10);
+    sendTgtVel(1,rpmA+robot_angle);
+    sendTgtVel(2,rpmA+robot_angle);
+    sendTgtVel(3,rpmA*(-1)+robot_angle);
+    sendTgtVel(4,rpmA*(-1)+robot_angle);
+    //指令値を送信
+    for(int i=1;i<= 4;i++){
+        sendCtrlEN(i);
+    }
+}
+
+
+void vel_forward_con(int rpmA){
+    int dis1 = get_cm_n(u1,15);
+    int dis2 = get_cm_n(u2,15);
+    
+    //速度を指定
+    int robot_angle = ((dis1 - dis2)*10);
     sendTgtVel(1,rpmA*(-1)+robot_angle);
     sendTgtVel(2,rpmA*(-1)+robot_angle);
     sendTgtVel(3,rpmA+robot_angle);