Diff: main.cpp

Revision:

8:0497ad6e03a4

Parent:

7:df29c4de6522

--- a/main.cpp	Sun Dec 19 18:48:16 2021 +0000
+++ b/main.cpp	Sun Dec 19 20:24:17 2021 +0000
@@ -74,7 +74,7 @@
 void set_MODE_T(void);
 
 void vel_stop(void);
-void vel_forward(int);
+void vel_forward(int,int,int);
 void vel_backward(int);
 void vel_right(int);
 void vel_left(int);
@@ -224,7 +224,10 @@
                 set_ACC(ACC_B);//加速度設定
                 set_DEC(ACC_C);//減速度設定
                 set_MODE_V();//速度制御モード送信
-                vel_forward(rpm);//前進速度指令
+               dist1 = get_cm_n(u1, 5);
+                dist2 = get_cm_n(u2, 5);
+
+                vel_forward(rpm,dist1,dist2);//前進速度指令
             }
         }else if(state_1 == 2){//前進からの帰還フェーズ
             if(ActPos > -3000){
@@ -302,12 +305,13 @@
     }
 }
 
-void vel_forward(int rpm){
+void vel_forward(int rpmA,int dis1,int dis2){
     //速度を指定
-    sendTgtVel(1,rpm*(-1));
-    sendTgtVel(2,rpm*(-1));
-    sendTgtVel(3,rpm);
-    sendTgtVel(4,rpm);
+    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);