![](/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:
- 6:e7b9b1d46852
- Parent:
- 5:aee4416b0d4f
- Child:
- 7:2e578dbe3a81
diff -r aee4416b0d4f -r e7b9b1d46852 main.cpp --- a/main.cpp Mon Oct 25 08:01:27 2021 +0000 +++ b/main.cpp Mon Oct 25 08:29:01 2021 +0000 @@ -87,7 +87,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.01,0.00,0.0,0)->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.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); @@ -118,7 +118,7 @@ /*--------------LOOP--------------*/ while(true){ hoge = x; - pos = RBT.getStatus(true); + pos = RBT.getStatus(false); bno.get_angles(); gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); gyroA(gyro); @@ -129,11 +129,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) { + if(!once and pos.x != 0.0 and pos.y != 0.0) { RBT.setPosition(0.0,0.0,0.0); once = true; } - RBT.setPosition(pos.x,pos.y,theta); + //RBT.setPosition(pos.x,pos.y,theta); //RBT.setVelocity(0.0,0.0,0.0); if(n<POINTS and @@ -164,7 +164,7 @@ v = 0.0; vel.theta = RBT.PIDs[0]->getmv(); } - else if(n == ISLAND and incircle(pos,NextP,40.0)){v = 0.0;vel.theta = 0.0;/*RBT.sendPWM(1,9)*/;} + else if(n == ISLAND and incircle(pos,NextP,40.0)){v = 0.0;vel.theta = 0.0;/*RBT.sendPWM(1,9);*/} 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;