2021年地区大会船ロボット「明石工船」 import時ライブラリの更新をしないでください
Dependencies: mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase
Diff: main.cpp
- Revision:
- 1:aa9cc4250220
- Parent:
- 0:f80bb69e638e
- Child:
- 2:4e7ec0816a91
diff -r f80bb69e638e -r aa9cc4250220 main.cpp --- a/main.cpp Wed Oct 13 08:47:48 2021 +0000 +++ b/main.cpp Thu Oct 21 16:16:24 2021 +0000 @@ -10,7 +10,7 @@ /*--------------FUNCTION--------------*/ -void init(); +bool init(); void route_read(); void getPoint(Position *p,int n); void getVelocity(double *v,int n); @@ -41,10 +41,12 @@ Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02); int main(){ + +PwmOut ok(PC_6); /*--------------SETUP--------------*/ -scrp.addCMD(6,checkMOT12); -scrp.addCMD(7,checkMOT34); +scrp.addCMD(7,checkMOT12); +scrp.addCMD(8,checkMOT34); AKASHIKOSEN.setCWID(1,2,3,4); AKASHIKOSEN.setSWID(5,6,7,8); @@ -61,14 +63,23 @@ 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.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); +RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5); +RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5); +RBT.addPID(0.0048,0.00,0.00,4)->setLimit(0.5,-0.5); RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); +*/ +MOT12 = false,MOT34 = false; -init(); - +while(init()); +printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34); route_read(); wait(1); - +ok = 1.0; bool setup = false; if(!setup){ @@ -78,9 +89,10 @@ } RBT.START(); +RBT.setPosition(0.0,0.0,0.0); /*--------------LOOP--------------*/ while(true){ - pos = RBT.getStatus(); + pos = RBT.getStatus(true); bno.get_angles(); gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); @@ -97,7 +109,7 @@ //RBT.setVelocity(0.0,0.0,0.0); if(n<POINTS and - ( (incircle(pos,NextP,30.0) and n < POINTS and n != 1000) or + ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000) or ( incircle(pos,NextP,10.0) and n == 1000 ) 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) ) @@ -105,19 +117,21 @@ { n++; getPoint(&NextP,n); + NextP.theta -= M_PI/2.0; getVelocity(&v,n-1); if(n < POINTS)getPoint(&subP,n+1); - if(v > 0.0) v *= 1.7; + if(v > 0.0) v *= 1.5; if(v >= 1400) v = 1400; - //v = 200.0; - if(n == POINTS) v = 50.0; + //v = 20.0; + if(n == POINTS) v = 20.0; printf("%d:pass\n",n); } if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0; double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x); - RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02); - RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getGain()); - //RBT.sendVelocity(200.0,200.0,0.0); + RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false); + RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getmv()); + //printf("%lf,%lf\n",pos.theta,NextP.theta); + //RBT.sendVelocity(200.0,-200.0,0.0); printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n); RBT.LOOP(); } @@ -136,8 +150,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); - y_info.push_back(y); + x_info.push_back(x*2.0); + y_info.push_back(y*1.5); v_info.push_back(v); theta_info.push_back(theta_); } @@ -199,8 +213,10 @@ return true; } -void init(){ - MOT12 = false,MOT34 = false; +bool init(){ + wait(0.25); scrp.send1(255,5,1); - while(!MOT12 or !MOT34); + wait(0.25); + scrp.send1(255,6,1); + return (!MOT12 or !MOT34); } \ No newline at end of file