2021年地区大会船ロボット「明石工船」 import時ライブラリの更新をしないでください
Dependencies: mbed arrc_mbed BNO055 take_mbed_old SDFileSystem ros_lib_melodic_buffer_increase
Diff: main.cpp
- Revision:
- 0:f80bb69e638e
- Child:
- 1:aa9cc4250220
diff -r 000000000000 -r f80bb69e638e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 13 08:47:48 2021 +0000 @@ -0,0 +1,206 @@ +#define USESCRP +#include "core.hpp" +#include "SDFileSystem.h" +#include "BNO055.h" +#include <cmath> +#include <vector> + +#include "mbed.h" +#define POINTS 900 + + +/*--------------FUNCTION--------------*/ +void 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(){ + +/*--------------SETUP--------------*/ +scrp.addCMD(6,checkMOT12); +scrp.addCMD(7,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.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); + +init(); + +route_read(); + +wait(1); + + +bool setup = false; +if(!setup){ + bno.reset(); + bno.setmode(OPERATION_MODE_IMUPLUS); + setup = true; +} + +RBT.START(); +/*--------------LOOP--------------*/ +while(true){ + pos = RBT.getStatus(); + + 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,30.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); + getVelocity(&v,n-1); + if(n < POINTS)getPoint(&subP,n+1); + if(v > 0.0) v *= 1.7; + if(v >= 1400) v = 1400; + //v = 200.0; + if(n == POINTS) v = 50.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); + 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); + y_info.push_back(y); + 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; +} + +void init(){ + MOT12 = false,MOT34 = false; + scrp.send1(255,5,1); + while(!MOT12 or !MOT34); +} \ No newline at end of file