2021年地区大会船ロボット「明石工船」 import時ライブラリの更新をしないでください
Dependencies: mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase
Diff: main.cpp
- Revision:
- 8:49f527ad271c
- Parent:
- 7:2e578dbe3a81
- Child:
- 9:9789153af55d
diff -r 2e578dbe3a81 -r 49f527ad271c main.cpp --- a/main.cpp Tue Oct 26 15:55:52 2021 +0000 +++ b/main.cpp Thu Oct 28 08:53:16 2021 +0000 @@ -8,8 +8,10 @@ #include <std_msgs/Int16.h> #include "mbed.h" -#define POINTS 900 -#define ISLAND 10 +#define POINTS 700 +#define ISLAND 702 +const double X0 = 442.5; +const double Y0 = 628.5; /*--------------FUNCTION--------------*/ @@ -92,7 +94,7 @@ 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.008,0.00,0.0,0)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.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); /* RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5); @@ -112,6 +114,8 @@ bool setup = false; bool once = false; + bool xy_stop = false; + bool rad_stop = false; if(!setup){ bno.reset(); bno.setmode(OPERATION_MODE_IMUPLUS); @@ -123,7 +127,7 @@ /*--------------LOOP--------------*/ while(true){ hoge = x; - pos = RBT.getStatus(false); + pos = RBT.getStatus(true); bno.get_angles(); gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); gyroA(gyro); @@ -134,11 +138,11 @@ else if(g_pulse < 0.0) theta += gyro-2.0*M_PI; else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro; else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI; - if(!once and pos.x != 0.0 and pos.y != 0.0) { - RBT.setPosition(0.0,0.0,0.0); + RBT.setPosition(pos.x,pos.y,theta); + if(!once){ + RBT.setPosition(X0,Y0,0.0); once = true; - } - //RBT.setPosition(pos.x,pos.y,theta); + } //RBT.setVelocity(0.0,0.0,0.0); if(n<POINTS and @@ -150,10 +154,10 @@ { n++; getPoint(&NextP,n); - NextP.theta -= M_PI/2.0; + //NextP.theta -= M_PI/2.0; getVelocity(&v,n-1); if(n < POINTS)getPoint(&subP,n+1); - if(v > 0.0) v *= 1.5; + if(v > 0.0) v *= 1.0; if(v >= 1400) v = 1400; //v = 20.0; if(n == POINTS) v = 20.0; @@ -164,21 +168,36 @@ RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false); vel.theta = RBT.PIDs[5]->getmv(); + xy_stop = false; + rad_stop = false; - if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 40){ - RBT.PIDs[0]->Update(pixel,0,0.02,false); - v = 0.0; + RBT.PIDs[0]->Update(pixel,0,0.02,false); + if(n == ISLAND and incircle(pos,NextP,30.0) and abs(pixel) > 40){ + xy_stop = true; vel.theta = RBT.PIDs[0]->getmv(); servo.pulsewidth_us(1500); cnt = 0; } - else if(n == ISLAND and incircle(pos,NextP,40.0)){cnt++;v = 0.0;vel.theta = 0.0;if((double)cnt > 3.0/0.02) servo.pulsewidth_us(1200);} + /* + if(n == ISLAND and incircle(pos,NextP,30.0) and pixel < -40){ + xy_stop = true; + vel.theta = 0.5; + cnt = 0; + } + else if(n == ISLAND and incircle(pos,NextP,30.0) and pixel > 40){ + xy_stop = true; + vel.theta = -0.5; + cnt = 0; + }*/ + else if(n == ISLAND and incircle(pos,NextP,30.0)){cnt++;xy_stop = true;rad_stop = true;if((double)cnt > 3.0/0.02) servo.pulsewidth_us(1170);} - if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0; - if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) vel.theta = 0.0; + if(n == POINTS and incircle(pos,NextP,1.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(xy_stop) {vel.x = 0.0;vel.y = 0.0;} + if(rad_stop) {vel.theta = 0.0;} RBT.sendVelocity(vel.x,vel.y,vel.theta); printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n); @@ -200,8 +219,8 @@ printf("Good!\n"); while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) { printf("%lf %lf %lf %lf\n",x, y, v, theta_); - x_info.push_back(x*2.0); - y_info.push_back(y*1.5); + x_info.push_back(x*1.0); + y_info.push_back(y*1.0); v_info.push_back(v); theta_info.push_back(theta_); }