Revision 9:5edd5ab59a99, committed 2021-12-19

Comitter:

tks1

Date:

Sun Dec 19 20:52:27 2021 +0000

Parent:

8:c9a5ef6f003f

Commit message:

p con add

Changed in this revision

diff -r c9a5ef6f003f -r 5edd5ab59a99 main.cpp
--- a/main.cpp	Sun Dec 19 20:44:45 2021 +0000
+++ b/main.cpp	Sun Dec 19 20:52:27 2021 +0000
@@ -76,7 +76,9 @@
 
 void vel_stop(void);
 void vel_forward(int);
+void vel_forward_con(int);
 void vel_backward(int);
+void vel_backward_con(int);
 void vel_right(int);
 void vel_left(int);
 
@@ -227,7 +229,7 @@
                 set_ACC(ACC_B);//加速度設定
                 set_DEC(ACC_C);//減速度設定
                 set_MODE_V();//速度制御モード送信
-                vel_forward(rpm);//前進速度指令
+                vel_forward_con(rpm);//前進速度指令
                 LED.printf("up\n\r");
             }
         }else if(state_1 == 2){//前進からの帰還フェーズ
@@ -237,7 +239,7 @@
                 wait(1.0);
                 //break;
             }else{
-                vel_backward(rpm);
+                vel_backward_con(rpm);
                 LED.printf("down\n\r");
             }
         }else if(state_1 == 11){//右進→壁検出フェーズ
@@ -309,6 +311,23 @@
     }
 }
 
+void vel_backward_con(int rpmA){
+    int dis1 = get_cm_n(u1,5);
+    int dis2 = get_cm_n(u2,5);
+    
+    //速度を指定
+    int robot_angle = ((dis1 - dis2)*50);
+    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(int rpm){
     //速度を指定
     sendTgtVel(1,rpm*(-1));
@@ -320,6 +339,23 @@
         sendCtrlEN(i);
     }
 }
+
+void vel_forward_con(int rpmA){
+    int dis1 = get_cm_n(u1,5);
+    int dis2 = get_cm_n(u2,5);
+    
+    //速度を指定
+    int robot_angle = ((dis1 - dis2)*50);
+    sendTgtVel(1,rpmA*(-1)+robot_angle);
+    sendTgtVel(2,rpmA*(-1)+robot_angle);
+    sendTgtVel(3,rpmA+robot_angle);
+    sendTgtVel(4,rpmA+robot_angle);
+    //指令値を送信
+    for(int i=1;i<= 4;i++){
+        sendCtrlEN(i);
+    }
+}
+
  
 void set_ACC(int setACC_val){
     sendProAcc(1,setACC_val);