![](/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:
- 7:2e578dbe3a81
- Parent:
- 6:e7b9b1d46852
- Child:
- 8:49f527ad271c
diff -r e7b9b1d46852 -r 2e578dbe3a81 main.cpp --- a/main.cpp Mon Oct 25 08:29:01 2021 +0000 +++ b/main.cpp Tue Oct 26 15:55:52 2021 +0000 @@ -9,7 +9,7 @@ #include "mbed.h" #define POINTS 900 -#define ISLAND 50 +#define ISLAND 10 /*--------------FUNCTION--------------*/ @@ -31,6 +31,7 @@ std::vector<double> t_info,x_info,y_info,v_info,theta_info; Position pos,vel,NextP,subP; int n=1; +int cnt = 0; double v=0.0; double theta = 0.0; double gyro = 0.0; @@ -59,10 +60,14 @@ DigitalOut hoge(PB_2); hoge = x; + PwmOut servo(PA_0); PwmOut ok(PC_6); /*--------------SETUP--------------*/ + servo.period_ms(2048); + servo.pulsewidth_us(1500); + nh.getHardware()->setBaud(115200); nh.initNode(); nh.subscribe(sub); @@ -159,12 +164,15 @@ RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false); vel.theta = RBT.PIDs[5]->getmv(); - if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 10){ + + 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; vel.theta = RBT.PIDs[0]->getmv(); + servo.pulsewidth_us(1500); + cnt = 0; } - 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)){cnt++;v = 0.0;vel.theta = 0.0;if((double)cnt > 3.0/0.02) servo.pulsewidth_us(1200);} 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;