![](/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:
- 5:aee4416b0d4f
- Parent:
- 4:0038da6cdc9a
- Child:
- 6:e7b9b1d46852
diff -r 0038da6cdc9a -r aee4416b0d4f main.cpp --- a/main.cpp Sun Oct 24 04:48:27 2021 +0000 +++ b/main.cpp Mon Oct 25 08:01:27 2021 +0000 @@ -45,14 +45,20 @@ Ticker rosspin; SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs -ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); +//ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); +ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,0x0807f800); Robot AKASHIKOSEN(50.8,25.4,322.5,259.75); Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02); -ros::Subscriber<std_msgs::Int16> sub("pixel", &getPixel); +ros::Subscriber<std_msgs::Int16> sub("unti", &getPixel); + +int x=1; int main(){ - + + DigitalOut hoge(PB_2); + hoge = x; + PwmOut ok(PC_6); /*--------------SETUP--------------*/ @@ -60,7 +66,7 @@ nh.getHardware()->setBaud(115200); nh.initNode(); nh.subscribe(sub); - rosspin.attach_us(&spinonce,100); + rosspin.attach_us(&spinonce,1000); scrp.addCMD(7,checkMOT12); scrp.addCMD(8,checkMOT34); @@ -100,6 +106,7 @@ ok = 1.0; bool setup = false; + bool once = false; if(!setup){ bno.reset(); bno.setmode(OPERATION_MODE_IMUPLUS); @@ -110,6 +117,7 @@ RBT.setPosition(0.0,0.0,0.0); /*--------------LOOP--------------*/ while(true){ + hoge = x; pos = RBT.getStatus(true); bno.get_angles(); gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); @@ -121,14 +129,17 @@ 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) { + RBT.setPosition(0.0,0.0,0.0); + once = true; + } RBT.setPosition(pos.x,pos.y,theta); //RBT.setVelocity(0.0,0.0,0.0); if(n<POINTS and ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000 and n != ISLAND) 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) + ( 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) ) ) { @@ -148,11 +159,12 @@ RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false); vel.theta = RBT.PIDs[5]->getmv(); - if(n == ISLAND and incircle(pos,NextP,10.0) and abs(pixel) > 10){ + if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 10){ RBT.PIDs[0]->Update(pixel,0,0.02,false); + v = 0.0; vel.theta = RBT.PIDs[0]->getmv(); } - else {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; @@ -162,6 +174,7 @@ 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); + nh.spinOnce(); RBT.LOOP(); } } @@ -247,6 +260,7 @@ scrp.send1(255,5,1); wait(0.25); scrp.send1(255,6,1); + printf("aaaa"); return (!MOT12 or !MOT34); } void spinonce() @@ -256,4 +270,9 @@ void getPixel(const std_msgs::Int16& pix){ pixel = pix.data; + if (pixel > 0){ + x = 0; + } else { + x = 1; } +}