Kanta Takasu
/
YUKA_ak_home_pcon
add pcon f and b
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
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);