![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2021年地区大会船ロボット「明石工船」 import時ライブラリの更新をしないでください
Dependencies: mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase
Diff: main.cpp
- Revision:
- 11:05c132be4f82
- Parent:
- 10:5c2adb59a0f0
- Child:
- 12:ed227eb82214
diff -r 5c2adb59a0f0 -r 05c132be4f82 main.cpp --- a/main.cpp Sat Oct 30 04:50:17 2021 +0000 +++ b/main.cpp Sat Oct 30 11:14:05 2021 +0000 @@ -95,7 +95,7 @@ RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5); RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5); RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5); - RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); + RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/4.0,-2.0*M_PI/4.0); RBT.addPID(0.007,0.00,0.0,0)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); //RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); /* @@ -150,7 +150,7 @@ //RBT.setVelocity(0.0,0.0,0.0); if(n<POINTS and - ( (incircle(pos,NextP,40.0) and n < POINTS and n != ISLAND and n != PARK) or + ( (incircle(pos,NextP,50.0) and n < POINTS and n != ISLAND and n != PARK) or ( incircle(pos,NextP,10.0) and n == PARK-1 ) or ( sqrt( (NextP.x-pos.x)*(NextP.x-pos.x)+(NextP.y-pos.y)*(NextP.y-pos.y) ) > sqrt( (subP.x-pos.x)*(subP.x-pos.x)+(subP.y-pos.y)*(subP.y-pos.y) ) and n < POINTS and n != ISLAND and n != PARK) ) @@ -162,7 +162,11 @@ getVelocity(&v,n-1); if(n < POINTS)getPoint(&subP,n+1); if(v > 0.0) v *= 1.0; - if(n < ISLAND) v*= 1.5; + if(n < PARK) v*= 1.9; + if(n < 120 and n > PARK) v*= 2.0; + if(160 > n and n >= 120) v*= 1.7; + if(180 > n and n >= 160) v*= 2.5; + if(n >= 180) v*= 3.0; //if(n > 700) v = 300; //v = 20.0; if(n == POINTS) v = 100.0; @@ -187,11 +191,11 @@ cnt++; xy_stop = true; rad_stop = true; - if((double)cnt > 2.0/0.02) {v=150.0;servo.pulsewidth_us(1170);n++;} + if((double)cnt > 2.0/0.02) {v=300.0;servo.pulsewidth_us(1170);n++;} } if(n > ISLAND and n <= ISLAND + 5) { cnt++; - if((double)cnt < 4.0/0.02) {v=150.0;servo.pulsewidth_us(1170);xy_stop = true;rad_stop = true;} + if((double)cnt < 4.0/0.02) {v=300.0;servo.pulsewidth_us(1170);xy_stop = true;rad_stop = true;} } if(n == POINTS and incircle(pos,NextP,10.0)) xy_stop = true; @@ -201,7 +205,7 @@ vel.y = v*sin(etheta); if(n == PARK and Lim_R == 1 and Lim_L == 1) {vel.x = 100.0; vel.y = 0.0; ok = 1;cnt = 0;} - else if(n == PARK) {xy_stop = true;rad_stop = true;ok = 0;cnt++;if((double)cnt > 12.0/0.02) {v=150.0;cnt = 0;n++;}} + else if(n == PARK) {xy_stop = true;rad_stop = true;ok = 0;cnt++;if((double)cnt > 10.0/0.02) {v=300.0;cnt = 0;n++;}} if(xy_stop) {vel.x = 0.0;vel.y = 0.0;} if(rad_stop) {vel.theta = 0.0;}