![](/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:
- 10:5c2adb59a0f0
- Parent:
- 9:9789153af55d
- Child:
- 11:05c132be4f82
diff -r 9789153af55d -r 5c2adb59a0f0 main.cpp --- a/main.cpp Fri Oct 29 16:31:25 2021 +0000 +++ b/main.cpp Sat Oct 30 04:50:17 2021 +0000 @@ -8,9 +8,9 @@ #include <std_msgs/Int16.h> #include "mbed.h" -#define POINTS 1900 -#define ISLAND 1898 -#define PARK 700 +#define POINTS 220 +#define ISLAND 190 +#define PARK 70 const double X0 = 442.5; const double Y0 = 628.5; @@ -42,7 +42,7 @@ bool gyro_1 = false,gyro_2 = false; long long g_pulse; bool MOT12 = false,MOT34 = false; - +bool ros_ = false; BNO055 bno(PB_3, PB_10); ros::NodeHandle nh; @@ -69,7 +69,6 @@ DigitalIn Lim_L(PA_4,PullDown); /*--------------SETUP--------------*/ - servo.period_ms(2048); servo.pulsewidth_us(1500); @@ -110,6 +109,7 @@ while(init()); printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34); + route_read(); wait(1); @@ -128,6 +128,7 @@ RBT.START(); RBT.setPosition(0.0,0.0,0.0); /*--------------LOOP--------------*/ + while(Lim_R == 1 and Lim_L == 1){}; while(true){ hoge = x; pos = RBT.getStatus(true); @@ -149,21 +150,22 @@ //RBT.setVelocity(0.0,0.0,0.0); if(n<POINTS and - ( (incircle(pos,NextP,60.0) and n < POINTS and n != ISLAND and n != PARK) or - ( incircle(pos,NextP,10.0) and n == 2000 ) or + ( (incircle(pos,NextP,40.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) ) ) { n++; getPoint(&NextP,n); - if(n < 100) NextP.theta = 0.0; + if(n < 10) NextP.theta = 0.0; 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 > 700) v = 300; //v = 20.0; - if(n == POINTS) v = 20.0; + if(n == POINTS) v = 100.0; printf("%d:pass\n",n); } @@ -175,7 +177,7 @@ rad_stop = false; RBT.PIDs[0]->Update(pixel,0.1,0.02,false); - servo.pulsewidth_us(1500); + if(n < ISLAND or n == POINTS) servo.pulsewidth_us(1500); if(n == ISLAND and incircle(pos,NextP,30.0) and abs(pixel) > 40){ xy_stop = true; vel.theta = RBT.PIDs[0]->getmv(); @@ -185,18 +187,21 @@ cnt++; xy_stop = true; rad_stop = true; - if((double)cnt > 2.5/0.02)n++; - if((double)cnt > 2.0/0.02) servo.pulsewidth_us(1170); + if((double)cnt > 2.0/0.02) {v=150.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(n == POINTS and incircle(pos,NextP,1.0)) xy_stop = true; + if(n == POINTS and incircle(pos,NextP,10.0)) xy_stop = true; if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) rad_stop = true;; vel.x = v*cos(etheta); 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 > 10.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 > 12.0/0.02) {v=150.0;cnt = 0;n++;}} if(xy_stop) {vel.x = 0.0;vel.y = 0.0;} if(rad_stop) {vel.theta = 0.0;} @@ -300,6 +305,7 @@ void getPixel(const std_msgs::Int16& pix){ pixel = pix.data; + ros_ = true; if (pixel > 0){ x = 0; } else {