![](/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@1:aa9cc4250220, 2021-10-21 (annotated)
- Committer:
- hamohamo
- Date:
- Thu Oct 21 16:16:24 2021 +0000
- Revision:
- 1:aa9cc4250220
- Parent:
- 0:f80bb69e638e
- Child:
- 2:4e7ec0816a91
sad
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hamohamo | 0:f80bb69e638e | 1 | #define USESCRP |
hamohamo | 0:f80bb69e638e | 2 | #include "core.hpp" |
hamohamo | 0:f80bb69e638e | 3 | #include "SDFileSystem.h" |
hamohamo | 0:f80bb69e638e | 4 | #include "BNO055.h" |
hamohamo | 0:f80bb69e638e | 5 | #include <cmath> |
hamohamo | 0:f80bb69e638e | 6 | #include <vector> |
hamohamo | 0:f80bb69e638e | 7 | |
hamohamo | 0:f80bb69e638e | 8 | #include "mbed.h" |
hamohamo | 0:f80bb69e638e | 9 | #define POINTS 900 |
hamohamo | 0:f80bb69e638e | 10 | |
hamohamo | 0:f80bb69e638e | 11 | |
hamohamo | 0:f80bb69e638e | 12 | /*--------------FUNCTION--------------*/ |
hamohamo | 1:aa9cc4250220 | 13 | bool init(); |
hamohamo | 0:f80bb69e638e | 14 | void route_read(); |
hamohamo | 0:f80bb69e638e | 15 | void getPoint(Position *p,int n); |
hamohamo | 0:f80bb69e638e | 16 | void getVelocity(double *v,int n); |
hamohamo | 0:f80bb69e638e | 17 | bool incircle(Position pos,Position target,double error); |
hamohamo | 0:f80bb69e638e | 18 | void gyroA(double theta); |
hamohamo | 0:f80bb69e638e | 19 | void gyroB(double theta); |
hamohamo | 0:f80bb69e638e | 20 | bool checkMOT12(int rx_data, int &tx_data); |
hamohamo | 0:f80bb69e638e | 21 | bool checkMOT34(int rx_data, int &tx_data); |
hamohamo | 0:f80bb69e638e | 22 | |
hamohamo | 0:f80bb69e638e | 23 | |
hamohamo | 0:f80bb69e638e | 24 | /*--------------VARIABLE--------------*/ |
hamohamo | 0:f80bb69e638e | 25 | |
hamohamo | 0:f80bb69e638e | 26 | std::vector<double> t_info,x_info,y_info,v_info,theta_info; |
hamohamo | 0:f80bb69e638e | 27 | Position pos,NextP,subP; |
hamohamo | 0:f80bb69e638e | 28 | int n=1; |
hamohamo | 0:f80bb69e638e | 29 | double v=0.0; |
hamohamo | 0:f80bb69e638e | 30 | double theta = 0.0; |
hamohamo | 0:f80bb69e638e | 31 | double gyro = 0.0; |
hamohamo | 0:f80bb69e638e | 32 | bool gyro_1 = false,gyro_2 = false; |
hamohamo | 0:f80bb69e638e | 33 | long long g_pulse; |
hamohamo | 0:f80bb69e638e | 34 | bool MOT12 = false,MOT34 = false; |
hamohamo | 0:f80bb69e638e | 35 | |
hamohamo | 0:f80bb69e638e | 36 | BNO055 bno(PB_3, PB_10); |
hamohamo | 0:f80bb69e638e | 37 | |
hamohamo | 0:f80bb69e638e | 38 | SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs |
hamohamo | 0:f80bb69e638e | 39 | ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800); |
hamohamo | 0:f80bb69e638e | 40 | Robot AKASHIKOSEN(50.8,25.4,322.5,259.75); |
hamohamo | 0:f80bb69e638e | 41 | Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02); |
hamohamo | 0:f80bb69e638e | 42 | |
hamohamo | 0:f80bb69e638e | 43 | int main(){ |
hamohamo | 1:aa9cc4250220 | 44 | |
hamohamo | 1:aa9cc4250220 | 45 | PwmOut ok(PC_6); |
hamohamo | 0:f80bb69e638e | 46 | |
hamohamo | 0:f80bb69e638e | 47 | /*--------------SETUP--------------*/ |
hamohamo | 1:aa9cc4250220 | 48 | scrp.addCMD(7,checkMOT12); |
hamohamo | 1:aa9cc4250220 | 49 | scrp.addCMD(8,checkMOT34); |
hamohamo | 0:f80bb69e638e | 50 | AKASHIKOSEN.setCWID(1,2,3,4); |
hamohamo | 0:f80bb69e638e | 51 | AKASHIKOSEN.setSWID(5,6,7,8); |
hamohamo | 0:f80bb69e638e | 52 | |
hamohamo | 0:f80bb69e638e | 53 | RBT.addENC(PC_4,PA_13,512,4,1); |
hamohamo | 0:f80bb69e638e | 54 | RBT.addENC(PA_14,PA_15,512,4,2); |
hamohamo | 0:f80bb69e638e | 55 | RBT.addENC(PC_2,PC_3,512,4,3); |
hamohamo | 0:f80bb69e638e | 56 | RBT.addENC(PC_10,PC_11,512,4,4); |
hamohamo | 0:f80bb69e638e | 57 | RBT.addENC(PA_7,PA_6,512,4,5); |
hamohamo | 0:f80bb69e638e | 58 | RBT.addENC(PA_9,PA_8,512,4,6); |
hamohamo | 0:f80bb69e638e | 59 | RBT.addENC(PC_1,PC_0,512,4,7); |
hamohamo | 0:f80bb69e638e | 60 | RBT.addENC(PC_5,PA_12,512,4,8); |
hamohamo | 0:f80bb69e638e | 61 | |
hamohamo | 0:f80bb69e638e | 62 | RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5); |
hamohamo | 0:f80bb69e638e | 63 | RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5); |
hamohamo | 0:f80bb69e638e | 64 | RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5); |
hamohamo | 0:f80bb69e638e | 65 | RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5); |
hamohamo | 1:aa9cc4250220 | 66 | RBT.addPID(0.8,0.00,0.0,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); |
hamohamo | 1:aa9cc4250220 | 67 | //RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); |
hamohamo | 1:aa9cc4250220 | 68 | /* |
hamohamo | 1:aa9cc4250220 | 69 | RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5); |
hamohamo | 1:aa9cc4250220 | 70 | RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5); |
hamohamo | 1:aa9cc4250220 | 71 | RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5); |
hamohamo | 1:aa9cc4250220 | 72 | RBT.addPID(0.0048,0.00,0.00,4)->setLimit(0.5,-0.5); |
hamohamo | 0:f80bb69e638e | 73 | RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0); |
hamohamo | 1:aa9cc4250220 | 74 | */ |
hamohamo | 1:aa9cc4250220 | 75 | MOT12 = false,MOT34 = false; |
hamohamo | 0:f80bb69e638e | 76 | |
hamohamo | 1:aa9cc4250220 | 77 | while(init()); |
hamohamo | 1:aa9cc4250220 | 78 | printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34); |
hamohamo | 0:f80bb69e638e | 79 | route_read(); |
hamohamo | 0:f80bb69e638e | 80 | |
hamohamo | 0:f80bb69e638e | 81 | wait(1); |
hamohamo | 1:aa9cc4250220 | 82 | ok = 1.0; |
hamohamo | 0:f80bb69e638e | 83 | |
hamohamo | 0:f80bb69e638e | 84 | bool setup = false; |
hamohamo | 0:f80bb69e638e | 85 | if(!setup){ |
hamohamo | 0:f80bb69e638e | 86 | bno.reset(); |
hamohamo | 0:f80bb69e638e | 87 | bno.setmode(OPERATION_MODE_IMUPLUS); |
hamohamo | 0:f80bb69e638e | 88 | setup = true; |
hamohamo | 0:f80bb69e638e | 89 | } |
hamohamo | 0:f80bb69e638e | 90 | |
hamohamo | 0:f80bb69e638e | 91 | RBT.START(); |
hamohamo | 1:aa9cc4250220 | 92 | RBT.setPosition(0.0,0.0,0.0); |
hamohamo | 0:f80bb69e638e | 93 | /*--------------LOOP--------------*/ |
hamohamo | 0:f80bb69e638e | 94 | while(true){ |
hamohamo | 1:aa9cc4250220 | 95 | pos = RBT.getStatus(true); |
hamohamo | 0:f80bb69e638e | 96 | |
hamohamo | 0:f80bb69e638e | 97 | bno.get_angles(); |
hamohamo | 0:f80bb69e638e | 98 | gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0); |
hamohamo | 0:f80bb69e638e | 99 | gyroA(gyro); |
hamohamo | 0:f80bb69e638e | 100 | gyroB(gyro); |
hamohamo | 0:f80bb69e638e | 101 | |
hamohamo | 0:f80bb69e638e | 102 | theta = 2.0*M_PI*(int)(g_pulse/4.0); |
hamohamo | 0:f80bb69e638e | 103 | if(g_pulse > 0.0) theta += gyro; |
hamohamo | 0:f80bb69e638e | 104 | else if(g_pulse < 0.0) theta += gyro-2.0*M_PI; |
hamohamo | 0:f80bb69e638e | 105 | else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro; |
hamohamo | 0:f80bb69e638e | 106 | else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI; |
hamohamo | 0:f80bb69e638e | 107 | |
hamohamo | 0:f80bb69e638e | 108 | RBT.setPosition(pos.x,pos.y,theta); |
hamohamo | 0:f80bb69e638e | 109 | |
hamohamo | 0:f80bb69e638e | 110 | //RBT.setVelocity(0.0,0.0,0.0); |
hamohamo | 0:f80bb69e638e | 111 | if(n<POINTS and |
hamohamo | 1:aa9cc4250220 | 112 | ( (incircle(pos,NextP,40.0) and n < POINTS and n != 1000) or |
hamohamo | 0:f80bb69e638e | 113 | ( incircle(pos,NextP,10.0) and n == 1000 ) or |
hamohamo | 0:f80bb69e638e | 114 | ( 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) |
hamohamo | 0:f80bb69e638e | 115 | ) |
hamohamo | 0:f80bb69e638e | 116 | ) |
hamohamo | 0:f80bb69e638e | 117 | { |
hamohamo | 0:f80bb69e638e | 118 | n++; |
hamohamo | 0:f80bb69e638e | 119 | getPoint(&NextP,n); |
hamohamo | 1:aa9cc4250220 | 120 | NextP.theta -= M_PI/2.0; |
hamohamo | 0:f80bb69e638e | 121 | getVelocity(&v,n-1); |
hamohamo | 0:f80bb69e638e | 122 | if(n < POINTS)getPoint(&subP,n+1); |
hamohamo | 1:aa9cc4250220 | 123 | if(v > 0.0) v *= 1.5; |
hamohamo | 0:f80bb69e638e | 124 | if(v >= 1400) v = 1400; |
hamohamo | 1:aa9cc4250220 | 125 | //v = 20.0; |
hamohamo | 1:aa9cc4250220 | 126 | if(n == POINTS) v = 20.0; |
hamohamo | 0:f80bb69e638e | 127 | printf("%d:pass\n",n); |
hamohamo | 0:f80bb69e638e | 128 | } |
hamohamo | 0:f80bb69e638e | 129 | if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0; |
hamohamo | 0:f80bb69e638e | 130 | double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x); |
hamohamo | 1:aa9cc4250220 | 131 | RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false); |
hamohamo | 1:aa9cc4250220 | 132 | RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getmv()); |
hamohamo | 1:aa9cc4250220 | 133 | //printf("%lf,%lf\n",pos.theta,NextP.theta); |
hamohamo | 1:aa9cc4250220 | 134 | //RBT.sendVelocity(200.0,-200.0,0.0); |
hamohamo | 0:f80bb69e638e | 135 | printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n); |
hamohamo | 0:f80bb69e638e | 136 | RBT.LOOP(); |
hamohamo | 0:f80bb69e638e | 137 | } |
hamohamo | 0:f80bb69e638e | 138 | } |
hamohamo | 0:f80bb69e638e | 139 | |
hamohamo | 0:f80bb69e638e | 140 | void route_read() { |
hamohamo | 0:f80bb69e638e | 141 | static double x, y, v, theta_; |
hamohamo | 0:f80bb69e638e | 142 | wait(1); |
hamohamo | 0:f80bb69e638e | 143 | printf("Hello World!\n"); |
hamohamo | 0:f80bb69e638e | 144 | wait(0.1); |
hamohamo | 0:f80bb69e638e | 145 | FILE *fp; |
hamohamo | 0:f80bb69e638e | 146 | do { |
hamohamo | 0:f80bb69e638e | 147 | fp = fopen("/sd/route.txt", "r"); |
hamohamo | 0:f80bb69e638e | 148 | wait(0.2); |
hamohamo | 0:f80bb69e638e | 149 | } while(fp==NULL); |
hamohamo | 0:f80bb69e638e | 150 | printf("Good!\n"); |
hamohamo | 0:f80bb69e638e | 151 | while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) { |
hamohamo | 0:f80bb69e638e | 152 | printf("%lf %lf %lf %lf\n",x, y, v, theta_); |
hamohamo | 1:aa9cc4250220 | 153 | x_info.push_back(x*2.0); |
hamohamo | 1:aa9cc4250220 | 154 | y_info.push_back(y*1.5); |
hamohamo | 0:f80bb69e638e | 155 | v_info.push_back(v); |
hamohamo | 0:f80bb69e638e | 156 | theta_info.push_back(theta_); |
hamohamo | 0:f80bb69e638e | 157 | } |
hamohamo | 0:f80bb69e638e | 158 | printf("checking...\n"); |
hamohamo | 0:f80bb69e638e | 159 | for(int k=0;k<x_info.size();k++){ |
hamohamo | 0:f80bb69e638e | 160 | printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]); |
hamohamo | 0:f80bb69e638e | 161 | } |
hamohamo | 0:f80bb69e638e | 162 | fclose(fp); |
hamohamo | 0:f80bb69e638e | 163 | } |
hamohamo | 0:f80bb69e638e | 164 | |
hamohamo | 0:f80bb69e638e | 165 | void getPoint(Position *p,int n){ |
hamohamo | 0:f80bb69e638e | 166 | p->x = x_info[n]; |
hamohamo | 0:f80bb69e638e | 167 | p->y = y_info[n]; |
hamohamo | 0:f80bb69e638e | 168 | p->theta = theta_info[n]; |
hamohamo | 0:f80bb69e638e | 169 | } |
hamohamo | 0:f80bb69e638e | 170 | |
hamohamo | 0:f80bb69e638e | 171 | void getVelocity(double *v,int n){ |
hamohamo | 0:f80bb69e638e | 172 | *v = v_info[n]; |
hamohamo | 0:f80bb69e638e | 173 | } |
hamohamo | 0:f80bb69e638e | 174 | |
hamohamo | 0:f80bb69e638e | 175 | bool incircle(Position pos,Position target,double error){ |
hamohamo | 0:f80bb69e638e | 176 | return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error); |
hamohamo | 0:f80bb69e638e | 177 | } |
hamohamo | 0:f80bb69e638e | 178 | void gyroA(double theta){ |
hamohamo | 0:f80bb69e638e | 179 | static bool prev = false; |
hamohamo | 0:f80bb69e638e | 180 | if(0 <= theta and theta < M_PI) gyro_1 = false; |
hamohamo | 0:f80bb69e638e | 181 | else gyro_1 = true; |
hamohamo | 0:f80bb69e638e | 182 | if(gyro_1 and !prev){ |
hamohamo | 0:f80bb69e638e | 183 | if(gyro_2) g_pulse++; |
hamohamo | 0:f80bb69e638e | 184 | else if(g_pulse != 0) g_pulse--; |
hamohamo | 0:f80bb69e638e | 185 | } |
hamohamo | 0:f80bb69e638e | 186 | else if(!gyro_1 and prev){ |
hamohamo | 0:f80bb69e638e | 187 | if(gyro_2) g_pulse--; |
hamohamo | 0:f80bb69e638e | 188 | else if(g_pulse != 0)g_pulse++; |
hamohamo | 0:f80bb69e638e | 189 | } |
hamohamo | 0:f80bb69e638e | 190 | prev = gyro_1; |
hamohamo | 0:f80bb69e638e | 191 | } |
hamohamo | 0:f80bb69e638e | 192 | void gyroB(double theta){ |
hamohamo | 0:f80bb69e638e | 193 | static bool prev = false; |
hamohamo | 0:f80bb69e638e | 194 | if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false; |
hamohamo | 0:f80bb69e638e | 195 | else gyro_2 = true; |
hamohamo | 0:f80bb69e638e | 196 | if(gyro_2 and !prev){ |
hamohamo | 0:f80bb69e638e | 197 | if(gyro_1) g_pulse--; |
hamohamo | 0:f80bb69e638e | 198 | else g_pulse++; |
hamohamo | 0:f80bb69e638e | 199 | } |
hamohamo | 0:f80bb69e638e | 200 | else if(!gyro_2 and prev){ |
hamohamo | 0:f80bb69e638e | 201 | if(gyro_1) g_pulse++; |
hamohamo | 0:f80bb69e638e | 202 | else g_pulse--; |
hamohamo | 0:f80bb69e638e | 203 | } |
hamohamo | 0:f80bb69e638e | 204 | prev = gyro_2; |
hamohamo | 0:f80bb69e638e | 205 | } |
hamohamo | 0:f80bb69e638e | 206 | |
hamohamo | 0:f80bb69e638e | 207 | bool checkMOT12(int rx_data, int &tx_data){ |
hamohamo | 0:f80bb69e638e | 208 | MOT12 = true; |
hamohamo | 0:f80bb69e638e | 209 | return true; |
hamohamo | 0:f80bb69e638e | 210 | } |
hamohamo | 0:f80bb69e638e | 211 | bool checkMOT34(int rx_data, int &tx_data){ |
hamohamo | 0:f80bb69e638e | 212 | MOT34 = true; |
hamohamo | 0:f80bb69e638e | 213 | return true; |
hamohamo | 0:f80bb69e638e | 214 | } |
hamohamo | 0:f80bb69e638e | 215 | |
hamohamo | 1:aa9cc4250220 | 216 | bool init(){ |
hamohamo | 1:aa9cc4250220 | 217 | wait(0.25); |
hamohamo | 0:f80bb69e638e | 218 | scrp.send1(255,5,1); |
hamohamo | 1:aa9cc4250220 | 219 | wait(0.25); |
hamohamo | 1:aa9cc4250220 | 220 | scrp.send1(255,6,1); |
hamohamo | 1:aa9cc4250220 | 221 | return (!MOT12 or !MOT34); |
hamohamo | 0:f80bb69e638e | 222 | } |