Kanta Takasu
/
YUKA_ak_home_pcon
add pcon f and b
Diff: main.cpp
- Revision:
- 4:e1aa0b348aeb
- Parent:
- 3:c39c14cfc811
- Child:
- 5:a7c3f446a1f1
diff -r c39c14cfc811 -r e1aa0b348aeb main.cpp --- a/main.cpp Sun Dec 19 11:16:07 2021 +0000 +++ b/main.cpp Sun Dec 19 15:43:05 2021 +0000 @@ -167,6 +167,7 @@ ActPos = 0; Init_Pos = 0; + dist1 = 0; readActPos(1); ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; if(ActPos > 8388608){ @@ -186,10 +187,16 @@ 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(); + printf("dsit1 %d\r\n",dist1); /*--------------------------*/ //前方向に入力があった時 - if(ActPos < Init_Pos - KICK){ + if(0){//ActPos < Init_Pos - KICK){ set_ACC(ACC_B); set_DEC(ACC_C); @@ -283,122 +290,9 @@ /*--------------------------*/ //右方向に入力があった時 - if(ActPos > Init_Pos + 2000){ - set_ACC(ACC_B); - set_DEC(ACC_C); - //速度制御モード送信 - set_MODE_V(); - - //速度を指定 - sendTgtVel(1,rpm); - sendTgtVel(2,rpm*(-1)); - sendTgtVel(3,rpm*(-1)); - sendTgtVel(4,rpm); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - //実行時間 - wait(2.0); - - //速度を指定 - sendTgtVel(1,0); - sendTgtVel(2,0); - sendTgtVel(3,0); - sendTgtVel(4,0); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - //実行時間 - wait(5.0); - - - //速度を指定 - sendTgtVel(1,rpm*(-1)); - sendTgtVel(2,rpm); - sendTgtVel(3,rpm); - sendTgtVel(4,rpm*(-1)); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - //実行時間 - wait(2.0); - - //ブレーキ - sendTgtVel(1,0); - sendTgtVel(2,0); - sendTgtVel(3,0); - sendTgtVel(4,0); - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - wait(0.5); - - set_MODE_T(); - - wait(4.0); - break; - } - //一定時間入力がない場合 - if(Time > Standby_Time){ - sendProAcc(1,5000); - sendProAcc(2,5000); - sendProAcc(3,5000); - sendProAcc(4,5000); - - sendProDec(1,5000); - sendProDec(2,5000); - sendProDec(3,5000); - sendProDec(4,5000); - - //速度制御モード送信 - set_MODE_V(); - - //速度を指定 - sendTgtVel(1,rpm); - sendTgtVel(2,rpm); - sendTgtVel(3,rpm); - sendTgtVel(4,rpm); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - //実行時間 - wait(1.0); - - //速度を指定 - sendTgtVel(1,rpm*(-1)); - sendTgtVel(2,rpm*(-1)); - sendTgtVel(3,rpm*(-1)); - sendTgtVel(4,rpm*(-1)); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - //実行時間 - wait(1.3); - - //ブレーキ - sendTgtVel(1,0); - sendTgtVel(2,0); - sendTgtVel(3,0); - sendTgtVel(4,0); - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } - - wait(0.5); - - set_MODE_T(); - - wait(5.0); - break; - } } - wait(0.1); + wait(0.5); } }