pyonta cansat
k
Dependencies: Hm MPL MPU60580 MU2Class SDFileSystem mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "xprintf.h" 00003 #include "MPL3115A2.h" 00004 #include "HMC5883L.h" 00005 #include "MPU6050.h" 00006 #include "SDFileSystem.h" 00007 #include "EthernetPowerControl.h" 00008 #define M_PI 3.141592 00009 00010 //hmc calibration mode 00011 #define STOP 0 //initial 00012 #define CAL 1 //calibration 00013 #define RUN 2 //run 00014 00015 //Sequence phase 00016 #define Preparing 1 00017 #define Rising 2 00018 #define Descending 3 00019 #define Landing_Fusing 4 00020 #define Moving 5 00021 00022 //Moving step 00023 #define get_goaldirection 0 00024 #define attitude_determination 1 00025 #define calculate_direction 2 00026 #define direction_control 3 00027 #define jump 4 00028 00029 DigitalOut myled1(LED1); 00030 DigitalOut myled2(LED2); 00031 DigitalOut myled3(LED3); 00032 DigitalOut myled4(LED4); 00033 00034 //ピンアサイン確認 00035 Serial gps(p13, p14); 00036 Serial Mu2(p28, p27); 00037 I2C i2c(p9, p10); // sda, scl 00038 Serial mp(USBTX, USBRX); 00039 SDFileSystem sd(p5, p6, p7, p8, "sd"); 00040 LocalFileSystem local("local"); 00041 DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター 00042 PwmOut pin21(p21); 00043 DigitalOut jumpmotor(p24); 00044 00045 //溶断 00046 DigitalOut para(p18); 00047 DigitalOut marker(p19); 00048 DigitalOut jumparm(p20); 00049 // 00050 DigitalOut Flightpin(p26); 00051 00052 Timer timer; 00053 int val; 00054 int val_total; 00055 unsigned char timer_set; 00056 unsigned char phase; 00057 unsigned char step; 00058 unsigned char pitch; 00059 00060 //気圧/////// 00061 MPL3115A2 APsensor(&i2c, &mp); 00062 int alt; 00063 int ini_alt; 00064 int min_alt; 00065 int min_alt5; 00066 int max_alt; 00067 int rela_max; 00068 int rela_ini; 00069 int flag1; 00070 int flag2; 00071 int flag3; 00072 unsigned char alttimes; 00073 void getAltitude(){ 00074 //wait_ms(300); 00075 Altitude a; 00076 Temperature t; 00077 Pressure p; 00078 APsensor.readAltitude(&a); 00079 APsensor.readTemperature(&t); 00080 APsensor.setModeStandby(); 00081 APsensor.setModeBarometer(); 00082 APsensor.setModeActive(); 00083 APsensor.readPressure(&p); 00084 alt = a; 00085 //気球試験用 00086 if(phase == Preparing){ 00087 rela_ini = alt - ini_alt; 00088 if(rela_ini > 50)flag1 = flag1+1; 00089 } 00090 00091 if(phase == Rising){ 00092 if(max_alt < alt){ 00093 max_alt = alt; 00094 } 00095 else{ 00096 rela_max = max_alt -alt; 00097 if(rela_max > 150)flag2 = flag2+1; 00098 } 00099 } 00100 //着地判定 00101 if(phase == Descending){ 00102 if (min_alt > alt){ 00103 min_alt = alt; 00104 min_alt5 = min_alt + 5; 00105 flag3 -= 1; 00106 } 00107 else { 00108 if( alt < min_alt5){ 00109 flag3 +=2; 00110 } 00111 } 00112 } 00113 mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase); 00114 mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max); 00115 mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3); 00116 //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure()); 00117 APsensor.setModeStandby(); 00118 APsensor.setModeAltimeter(); 00119 APsensor.setModeActive(); 00120 } 00121 ///// 00122 00123 //GPS 00124 int i,rlock,mode; 00125 char gps_data[256]; 00126 char ns,ew; 00127 float w_time,hokui,tokei; 00128 float g_hokui,g_tokei; 00129 float d_hokui,m_hokui,d_tokei,m_tokei; 00130 unsigned char c; 00131 //目標経度，緯度 00132 float goal_tokei,goal_hokui; 00133 float goal_direction; 00134 float direction_y; 00135 float direction_x; 00136 float angular_difference; 00137 00138 void getGPS(){ 00139 //目的地 ブラックロック修正 00140 goal_tokei = 119.1218; 00141 goal_hokui = 40.8797; 00142 c = gps.getc(); 00143 if( c=='$' || i == 256){ 00144 mode = 0; 00145 i = 0; 00146 for(int j=0; j<256; j++){ 00147 gps_data[j]=NULL; 00148 } 00149 } 00150 if(mode==0){ 00151 if((gps_data[i]=c) != '\r'){ 00152 i++; 00153 }else{ 00154 gps_data[i]='\0'; 00155 00156 if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ 00157 if(rlock==1){ 00158 //mp.printf("Status:Lock(%d)\n\r",rlock); 00159 //logitude 00160 d_tokei= int(tokei/100); 00161 m_tokei= (tokei-d_tokei*100)/60; 00162 g_tokei= d_tokei+m_tokei; 00163 //Latitude 00164 d_hokui=int(hokui/100); 00165 m_hokui=(hokui-d_hokui*100)/60; 00166 g_hokui=d_hokui+m_hokui; 00167 mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui); 00168 Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui); 00169 float longitudinalDifference =-(goal_tokei - g_tokei); 00170 float latitudinalDifference = goal_hokui - g_hokui; 00171 00172 //球面三角法 00173 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); 00174 direction_x = cos(g_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(g_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180); 00175 goal_direction = atan2f(direction_y,direction_x); 00176 if(goal_direction < 0)goal_direction += 2*M_PI; 00177 00178 //磁北に対する角度に調節 ネバダ東に14度 00179 goal_direction -= 0.244346; 00180 if (goal_direction < 0) goal_direction += 2*M_PI; 00181 mp.printf("goal:%f\n\r",goal_direction*180/M_PI); 00182 //mp.printf("longitudinalDifference:%f",longitudinalDifference); 00183 //mp.printf("latitudinalDifference:%f",latitudinalDifference); 00184 if(phase == Descending)getAltitude(); 00185 if(phase == Moving)step = attitude_determination; 00186 } 00187 else{ 00188 mp.printf("\n\rStatus:unLock(%d)\n\r",rlock); 00189 mp.printf("%s",gps_data); 00190 Mu2.printf("@DT02No\r\n"); 00191 if(phase == Descending)getAltitude(); 00192 if(phase == Moving)step = jump; 00193 } 00194 sprintf(gps_data, ""); 00195 } 00196 } 00197 } 00198 } 00199 00200 void MU2initialize(){ 00201 Mu2.baud(19200); 00202 Mu2.printf("@EI34\r\n"); 00203 wait(1); 00204 Mu2.printf("@DI25\r\n"); 00205 wait(1); 00206 Mu2.printf("@CH0F\r\n"); 00207 wait(1); 00208 Mu2.printf("@GI34\r\n"); 00209 wait(1); 00210 } 00211 00212 ///////////////////// 00213 //地磁気 00214 HMC5883L compass(p9, p10); 00215 Ticker interrupt; 00216 double heading0 = 0.0; 00217 double heading1 = 0.0; 00218 double heading2 = 0.0; 00219 double heading3 = 0.0; 00220 double headingLPF = 0.0; 00221 double initHeading; 00222 double tgtHeading; 00223 double preHeading = 0.0; 00224 unsigned char hmc_mode; 00225 00226 int maxX, minX, maxY, minY; 00227 int ofsX = 0; //calibration x 00228 int ofsY = 0; //calibration y 00229 int counter = 0; 00230 int ofsXset = 1122; 00231 int ofsYset = -466; 00232 int ofsZset = -522; 00233 00234 //地磁気キャリブレーション用 00235 void intrpt() { 00236 int16_t raw[3]; 00237 compass.getXYZ(raw); 00238 //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2] 00239 double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset)); 00240 if(heading < 0)heading += 2*M_PI; 00241 if(heading > 2*M_PI)heading -= 2*M_PI; 00242 heading3 = heading2; 00243 heading2 = heading1; 00244 heading1 = heading0; 00245 heading0 = heading; 00246 headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter 00247 00248 switch(hmc_mode) { 00249 case STOP: 00250 if(counter == 100) { 00251 initHeading = headingLPF; 00252 hmc_mode = CAL; 00253 maxX = raw[0]; 00254 minX = raw[0]; 00255 maxY = raw[2]; 00256 minY = raw[2]; 00257 counter = 0; 00258 //mp.printf("STOP end\n\r"); 00259 } 00260 break; 00261 case CAL: 00262 if(raw[0] > maxX)maxX = raw[0]; 00263 if(raw[0] < minX)minX = raw[0]; 00264 if(raw[2] > maxY)maxY = raw[2]; 00265 if(raw[2] < minY)minY = raw[2]; 00266 if((counter > 100) 00267 && (headingLPF > (initHeading-0.01)) 00268 && (headingLPF < (initHeading+0.01))) { 00269 ofsX = (maxX + minX)/2; 00270 ofsY = (maxY + minY)/2; 00271 //hmc_mode = RUN; 00272 counter = 0; 00273 //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY); 00274 } 00275 break; 00276 case RUN: 00277 mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset); 00278 //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]); 00279 //step = calculate_direction; 00280 break; 00281 00282 } 00283 counter++; 00284 } 00285 00286 //加速度 00287 MPU6050 mpu; 00288 int16_t ax, ay, az; 00289 int16_t gx, gy, gz; 00290 int32_t axa, aya, aza; 00291 int32_t gxa, gya, gza; 00292 00293 //オフセット値 00294 double axoffset = 586.711; 00295 double ayoffset = -174.847; 00296 double azoffset = -2195.34375; 00297 00298 //センサの出力あたりの加速度[m/s^2] 00299 double axrate = 0.000601615; 00300 double ayrate = 0.000598334; 00301 double azrate = 0.0005845; 00302 00303 //3軸キャリブレーション 00304 double phi; 00305 double theta; 00306 double heading3axis; 00307 00308 double bfy; 00309 double bfx; 00310 00311 00312 //3軸からheading算出 00313 void getheading3axis() { 00314 //加速度算出 00315 mpu.getAcceleration(&ax, &ay, &az); 00316 double axcal = -axrate * (static_cast<double>(ax) - axoffset); 00317 double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); 00318 double azcal = -azrate * (static_cast<double>(az) - azoffset); 00319 00320 phi = atan2(aycal,azcal); 00321 //phi += M_PI; 00322 theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi)); 00323 if(theta > 0)pitch = 1; 00324 if(theta < 0)pitch = 0; 00325 //theta += M_PI; 00326 00327 //地磁気算出 00328 int16_t raw[3]; 00329 compass.getXYZ(raw); 00330 bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi); 00331 bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta) 00332 + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi) 00333 + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi); 00334 double heading = atan2(-bfy, bfx); 00335 if(heading < 0)heading += 2*M_PI; 00336 if(heading > 2*M_PI)heading -= 2*M_PI; 00337 heading3 = heading2; 00338 heading2 = heading1; 00339 heading1 = heading0; 00340 heading0 = heading; 00341 headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter 00342 //headingLPF += 0.349066; 00343 if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI; 00344 //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal); 00345 mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI); 00346 } 00347 00348 //ADXL375 00349 const int addr = 0xA6; 00350 char adxl[6]; 00351 short int axout = 0; 00352 short int ayout = 0; 00353 short int azout = 0; 00354 00355 void adxl_init(){ 00356 char fifo[2] = {0x38,0x80}; 00357 i2c.write(addr,fifo,2); //FIFO_Mode -> Stream 00358 char power[2] = {0x2D,0x08}; 00359 i2c.write(addr,power,2); //measurebit -> 1 00360 } 00361 00362 void adxl_get(){ 00363 adxl[0] = 0x32; 00364 i2c.write(addr,adxl,1); 00365 i2c.read(0xA7,adxl,6); 00366 axout = (((int16_t )adxl[1]) << 8) | adxl[0]; 00367 ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; 00368 azout = (((int16_t )adxl[5]) << 8) | adxl[4]; 00369 } 00370 00371 00372 //モーター駆動方向制御 00373 void motor_drive(){ 00374 controlmotor1.write(1); 00375 controlmotor2.write(0); 00376 pin21.write(1.0); 00377 //if(pin1)mp.printf("Yes!\r\n"); 00378 } 00379 00380 //溶断プログラム 00381 //最終チェックポイント//////////////////////////////// 00382 void burning(){ 00383 wait(5); 00384 myled1 = 1; 00385 //para = 1; 00386 wait(1); 00387 myled1 = 0; 00388 //para = 0; 00389 wait(5); 00390 myled2 = 1; 00391 //marker = 1; 00392 wait(1); 00393 myled2 = 0; 00394 //marker = 0; 00395 wait(5); 00396 myled3 = 1; 00397 //jumparm = 1; 00398 wait(1); 00399 myled3 = 0; 00400 //jumparm = 0; 00401 } 00402 //跳躍モーター駆動まいまいかわいい 00403 void jumping(){ 00404 timer.start(); 00405 //モーター駆動時間調整 00406 while(val < 4){ 00407 val = timer.read(); 00408 jumpmotor = 1; 00409 myled4 != myled4; 00410 } 00411 timer.reset(); 00412 timer.stop(); 00413 jumpmotor = 0; 00414 val = 0; 00415 } 00416 00417 ///test 00418 //GPSなし方向制御，跳躍テスト 00419 float test_tokei,test_hokui; 00420 void test_get_direction(){ 00421 //目的地 大岡山駅 00422 goal_tokei = 139.686234; 00423 goal_hokui = 35.614330; 00424 00425 //テスト場所石川台一号館 00426 test_tokei = 139.681529; 00427 test_hokui = 35.605263; 00428 00429 float longitudinalDifference = goal_tokei - test_tokei; 00430 float latitudinalDifference = goal_hokui - test_hokui; 00431 //球面三角法 00432 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); 00433 direction_x = cos(test_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(test_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180); 00434 goal_direction = atan2f(direction_y,direction_x); 00435 if(goal_direction < 0)goal_direction += 2*M_PI; 00436 00437 //磁北に対する角度に調節 東京7度 00438 goal_direction += 0.122173; 00439 //goal_direction2 += 0.122173; 00440 if (goal_direction < 0) goal_direction += 2*M_PI; 00441 } 00442 00443 /////////////////////////main//////////////////////////////////// 00444 int main(){ 00445 PHY_PowerDown();//省電力 00446 wait(2); 00447 //センサ初期化 00448 Mu2.baud(19200); 00449 Mu2.printf("@EI34\r\n"); 00450 wait(1); 00451 Mu2.printf("@DI25\r\n"); 00452 wait(1); 00453 Mu2.printf("@CH0F\r\n"); 00454 wait(1); 00455 Mu2.printf("@GI34\r\n"); 00456 wait(1); 00457 APsensor.init(); 00458 wait(0.2); 00459 hmc_mode = RUN; 00460 mpu.initialize(); 00461 wait(0.2); 00462 compass.init(); 00463 wait(0.2); 00464 adxl_init(); 00465 wait(0.2); 00466 APsensor.setOffsetAltitude(100); 00467 APsensor.setOffsetTemperature(10); 00468 APsensor.setOffsetPressure(-32); 00469 mkdir("/sd/mydir", 0777); 00470 min_alt = 5000.0; 00471 Flightpin = 1; 00472 //高度初期値 00473 for(int h = 0; h < 10; h++){ 00474 getAltitude(); 00475 ini_alt = ini_alt + alt; 00476 wait(0.5); 00477 } 00478 ini_alt = ini_alt/10; 00479 mp.printf("ini_alt:%d\r\n",ini_alt); 00480 int jump_count; 00481 00482 //最終チェックポイント///////////////////////////////////////// 00483 //phase = Preparing; 00484 //wait(1500);//開始25分待機 00485 //phase = Descending; 00486 //phase = Moving; 00487 phase = Landing_Fusing; 00488 /////////////////////////////単機能//////// 00489 00490 //wait(5); 00491 00492 //while(1){ 00493 //myled1 = 1; 00494 //Flightpin = 1; 00495 //GPS_MU2 00496 //getGPS(); 00497 //方向制御用モーター 00498 /* 00499 controlmotor1.write(1); 00500 controlmotor2.write(0); 00501 pin21.write(0.8); 00502 wait(3); 00503 controlmotor1.write(0); 00504 controlmotor2.write(0); 00505 pin21.write(0.0); 00506 */ 00507 //跳躍用モーター 00508 //jumpmotor = 1; 00509 //wait(5); 00510 //jumpmotor = 0; 00511 //溶断 00512 //burning(); 00513 //気圧 00514 //getAltitude(); 00515 //mpu 00516 /* 00517 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 00518 double axcal = -axrate * (static_cast<double>(ax) - axoffset); 00519 double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); 00520 double azcal = -azrate * (static_cast<double>(az) - azoffset); 00521 mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal); 00522 */ 00523 //hmc 00524 /* 00525 hmc_mode = RUN; 00526 intrpt(); 00527 wait(0.5); 00528 */ 00529 //hmc_mpu 00530 //getheading3axis() ; 00531 //wait(0.3); 00532 //adxl_SDカード 跳躍高さ測定 00533 /* 00534 mkdir("/sd/mydir", 0777); 00535 wait(1); 00536 FILE *fp = fopen("/sd/mydir/height1.txt", "a"); 00537 wait(1); 00538 if(fp == NULL) { 00539 error("Could not open file for write\n"); 00540 } 00541 timer.reset(); 00542 timer.start(); 00543 val = 0; 00544 while(val < 5000){ 00545 jumpmotor = 1; 00546 val = timer.read_ms(); 00547 adxl[0] = 0x32; 00548 i2c.write(addr,adxl,1); 00549 i2c.read(0xA7,adxl,6); 00550 axout = (((int16_t )adxl[1]) << 8) | adxl[0]; 00551 ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; 00552 azout = (((int16_t )adxl[5]) << 8) | adxl[4]; 00553 fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 00554 wait_ms(2); 00555 myled1 !=myled1; 00556 } 00557 myled1 = 0; 00558 jumpmotor = 0; 00559 wait(1); 00560 fclose(fp); 00561 */ 00562 // 00563 00564 //} 00565 ///////////////////////// 00566 00567 //着地検知用timer 00568 timer.reset(); 00569 timer.start(); 00570 val = 0; 00571 while(1){ 00572 switch(phase){ 00573 case Preparing: 00574 myled1 = 1; 00575 getAltitude(); 00576 wait(0.5); 00577 val = timer.read(); 00578 if(flag1 > 20) phase = Rising; 00579 if( val > 600) phase = Rising; //計35分 00580 break; 00581 case Rising: 00582 myled1 = 0; 00583 myled2 = 1; 00584 getAltitude(); 00585 wait(0.5); 00586 val = timer.read(); 00587 if(flag2 > 40) phase = Descending; 00588 if(val > 1800) phase = Descending; //計55分 00589 break; 00590 case Descending: 00591 val = timer.read(); 00592 myled2 = 0; 00593 myled3 = 1; 00594 if(flag3 < 50){ 00595 getGPS(); 00596 } 00597 else{ 00598 getAltitude(); 00599 } 00600 if ( val >1500){ 00601 timer.reset(); 00602 val = 0; 00603 timer_set = 1; 00604 } 00605 if (timer_set == 1){ 00606 val_total = 1500 + val; 00607 } 00608 //最終チェックポイント////////////////////////////////////////// 00609 if ( flag3 >= 200){ 00610 if( val > 900){ 00611 phase = Landing_Fusing; 00612 } 00613 } 00614 if( val_total >2400 ){ 00615 wait(600); 00616 phase = Landing_Fusing;//電源onから75分 00617 } 00618 break; 00619 case Landing_Fusing: 00620 timer.reset(); 00621 timer.stop(); 00622 val = 0; 00623 myled3 = 0; 00624 myled4 = 1; 00625 //wait(30); 00626 burning(); 00627 wait(5); 00628 Mu2.printf("@DT04FIRE\r\n"); 00629 for ( int gp; gp <= 10; gp++){ 00630 getGPS(); 00631 } 00632 00633 wait(2); 00634 jumpmotor = 1; 00635 wait(10); 00636 jumpmotor = 0; 00637 wait(2); 00638 pin21.write(1.0); 00639 controlmotor1.write(0); 00640 controlmotor2.write(1); 00641 wait(4); 00642 pin21.write(0.0); 00643 controlmotor1.write(0); 00644 controlmotor2.write(0); 00645 wait(3); 00646 jumpmotor = 1; 00647 wait(10); 00648 jumpmotor = 0; 00649 phase = Moving; 00650 break; 00651 case Moving: 00652 myled2 = 0; 00653 myled1 = 0; 00654 myled3 = 0; 00655 myled4 = 0; 00656 wait(1); 00657 switch(step){ 00658 case get_goaldirection: 00659 myled1 = 1; 00660 getGPS(); 00661 step = attitude_determination; 00662 wait(1); 00663 break; 00664 case attitude_determination: 00665 myled1 = 0; 00666 myled2 = 1; 00667 timer.reset(); 00668 timer.start(); 00669 val = 0; 00670 while(val < 3){ 00671 val = timer.read(); 00672 getheading3axis(); 00673 wait(0.5); 00674 } 00675 timer.reset(); 00676 timer.stop(); 00677 val = 0; 00678 getheading3axis(); 00679 step = calculate_direction; 00680 wait(1); 00681 break; 00682 case calculate_direction: 00683 myled2 = 0; 00684 myled3 = 1; 00685 if(headingLPF < M_PI){ 00686 if(goal_direction < headingLPF+M_PI){ 00687 angular_difference = headingLPF - goal_direction; 00688 } 00689 else{ 00690 angular_difference = 2*M_PI + (headingLPF - goal_direction); 00691 } 00692 } 00693 else{ 00694 if(goal_direction < headingLPF - M_PI){ 00695 angular_difference = headingLPF - goal_direction - 2*M_PI; 00696 } 00697 else{ 00698 angular_difference = headingLPF - goal_direction; 00699 } 00700 } 00701 mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI); 00702 //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI); 00703 //40度以下でジャンプ 00704 if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){ 00705 step = jump; 00706 } 00707 else { 00708 step = direction_control; 00709 } 00710 if(pitch == 1) step = direction_control; 00711 wait(1); 00712 break; 00713 00714 case direction_control: 00715 myled1 = 0; 00716 myled3 = 0; 00717 myled4 = 1; 00718 //int drive_time; 00719 timer.reset(); 00720 timer.stop(); 00721 timer.start(); 00722 val = 0; 00723 pin21.write(1.0); 00724 if(pitch == 1){ 00725 controlmotor1.write(0); 00726 controlmotor2.write(1); 00727 }else{ 00728 controlmotor1.write(1); 00729 controlmotor2.write(0); 00730 } 00731 wait(3); 00732 controlmotor1.write(0); 00733 controlmotor2.write(0); 00734 pin21.write(0.0); 00735 timer.reset(); 00736 timer.stop(); 00737 val = 0; 00738 //step = jump; 00739 if(jump_count == 2){ 00740 step = jump; 00741 } 00742 else{ 00743 step = attitude_determination; 00744 } 00745 jump_count = jump_count+1; 00746 break; 00747 00748 case jump: 00749 //ログファイル 00750 myled1 = 1; 00751 myled2 = 1; 00752 wait(2); 00753 FILE *fp = fopen("/sd/mydir/height.txt", "a"); 00754 wait(1); 00755 timer.reset(); 00756 timer.start(); 00757 val = 0; 00758 jumpmotor = 1; 00759 Mu2.printf("@DT04JUMP\r\n"); 00760 while(val < 4000){ 00761 val = timer.read_ms(); 00762 if(fp == NULL) { 00763 jumpmotor = 1; 00764 } 00765 else{ 00766 adxl[0] = 0x32; 00767 i2c.write(addr,adxl,1); 00768 i2c.read(0xA7,adxl,6); 00769 axout = (((int16_t )adxl[1]) << 8) | adxl[0]; 00770 ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; 00771 azout = (((int16_t )adxl[5]) << 8) | adxl[4]; 00772 fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 00773 wait_ms(2); 00774 } 00775 } 00776 jumpmotor = 0; 00777 wait(1); 00778 if(fp != NULL)fclose(fp); 00779 myled3 = 1; 00780 val = 0; 00781 jump_count = 0; 00782 wait(3); 00783 step = get_goaldirection; 00784 break; 00785 } 00786 } 00787 } 00788 }
Generated on Mon Jul 18 2022 18:12:48 by
1.7.2