![](/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
main.cpp
- Committer:
- hamohamo
- Date:
- 2021-10-21
- Revision:
- 1:aa9cc4250220
- Parent:
- 0:f80bb69e638e
- Child:
- 2:4e7ec0816a91
File content as of revision 1:aa9cc4250220:
#define USESCRP #include "core.hpp" #include "SDFileSystem.h" #include "BNO055.h" #include <cmath> #include <vector> #include "mbed.h" #define POINTS 900 /*--------------FUNCTION--------------*/ bool init(); void route_read(); void getPoint(Position *p,int n); void getVelocity(double *v,int n); bool incircle(Position pos,Position target,double error); void gyroA(double theta); void gyroB(double theta); bool checkMOT12(int rx_data, int &tx_data); bool checkMOT34(int rx_data, int &tx_data); /*--------------VARIABLE--------------*/ std::vector<double> t_info,x_info,y_info,v_info,theta_info; Position pos,NextP,subP; int n=1; double v=0.0; double theta = 0.0; double gyro = 0.0; bool gyro_1 = false,gyro_2 = false; long long g_pulse; bool MOT12 = false,MOT34 = false; BNO055 bno(PB_3, PB_10); 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); Robot AKASHIKOSEN(50.8,25.4,322.5,259.75); Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02); int main(){ PwmOut ok(PC_6); /*--------------SETUP--------------*/ scrp.addCMD(7,checkMOT12); scrp.addCMD(8,checkMOT34); AKASHIKOSEN.setCWID(1,2,3,4); AKASHIKOSEN.setSWID(5,6,7,8); RBT.addENC(PC_4,PA_13,512,4,1); RBT.addENC(PA_14,PA_15,512,4,2); RBT.addENC(PC_2,PC_3,512,4,3); RBT.addENC(PC_10,PC_11,512,4,4); RBT.addENC(PA_7,PA_6,512,4,5); RBT.addENC(PA_9,PA_8,512,4,6); RBT.addENC(PC_1,PC_0,512,4,7); RBT.addENC(PC_5,PA_12,512,4,8); RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5); 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; while(init()); printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34); route_read(); wait(1); ok = 1.0; bool setup = false; if(!setup){ bno.reset(); bno.setmode(OPERATION_MODE_IMUPLUS); setup = true; } RBT.START(); RBT.setPosition(0.0,0.0,0.0); /*--------------LOOP--------------*/ while(true){ pos = RBT.getStatus(true); bno.get_angles(); gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); gyroA(gyro); gyroB(gyro); theta = 2.0*M_PI*(int)(g_pulse/4.0); if(g_pulse > 0.0) theta += gyro; 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; 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) 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) ) ) { 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.5; if(v >= 1400) v = 1400; //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,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(); } } void route_read() { static double x, y, v, theta_; wait(1); printf("Hello World!\n"); wait(0.1); FILE *fp; do { fp = fopen("/sd/route.txt", "r"); wait(0.2); } while(fp==NULL); 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); v_info.push_back(v); theta_info.push_back(theta_); } printf("checking...\n"); for(int k=0;k<x_info.size();k++){ printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]); } fclose(fp); } void getPoint(Position *p,int n){ p->x = x_info[n]; p->y = y_info[n]; p->theta = theta_info[n]; } void getVelocity(double *v,int n){ *v = v_info[n]; } bool incircle(Position pos,Position target,double error){ return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error); } void gyroA(double theta){ static bool prev = false; if(0 <= theta and theta < M_PI) gyro_1 = false; else gyro_1 = true; if(gyro_1 and !prev){ if(gyro_2) g_pulse++; else if(g_pulse != 0) g_pulse--; } else if(!gyro_1 and prev){ if(gyro_2) g_pulse--; else if(g_pulse != 0)g_pulse++; } prev = gyro_1; } void gyroB(double theta){ static bool prev = false; if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false; else gyro_2 = true; if(gyro_2 and !prev){ if(gyro_1) g_pulse--; else g_pulse++; } else if(!gyro_2 and prev){ if(gyro_1) g_pulse++; else g_pulse--; } prev = gyro_2; } bool checkMOT12(int rx_data, int &tx_data){ MOT12 = true; return true; } bool checkMOT34(int rx_data, int &tx_data){ MOT34 = true; return true; } bool init(){ wait(0.25); scrp.send1(255,5,1); wait(0.25); scrp.send1(255,6,1); return (!MOT12 or !MOT34); }