k
Dependencies: Hm MPL MPU60580 MU2Class SDFileSystem mbed
main.cpp
- Committer:
- pyonta2017
- Date:
- 2017-09-11
- Revision:
- 0:6ddf1386e71d
- Child:
- 1:8a25883c423c
File content as of revision 0:6ddf1386e71d:
#include "mbed.h" #include "xprintf.h" #include "MPL3115A2.h" #include "HMC5883L.h" #include "MPU6050.h" #include "SDFileSystem.h" #include "EthernetPowerControl.h" #define M_PI 3.141592 //hmc calibration mode #define STOP 0 //initial #define CAL 1 //calibration #define RUN 2 //run //Sequence phase #define Preparing 1 #define Rising 2 #define Descending 3 #define Landing_Fusing 4 #define Moving 5 //Moving step #define get_goaldirection 0 #define attitude_determination 1 #define calculate_direction 2 #define direction_control 3 #define jump 4 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); //ピンアサイン確認 Serial gps(p13, p14); Serial Mu2(p28, p27); I2C i2c(p9, p10); // sda, scl Serial mp(USBTX, USBRX); SDFileSystem sd(p5, p6, p7, p8, "sd"); LocalFileSystem local("local"); DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター PwmOut pin21(p21); DigitalOut jumpmotor(p24); //溶断 DigitalOut para(p18); DigitalOut marker(p19); DigitalOut jumparm(p20); // DigitalOut Flightpin(p26); Timer timer; int val; int val_total; unsigned char timer_set; unsigned char phase; unsigned char step; //気圧/////// MPL3115A2 APsensor(&i2c, &mp); int alt; int ini_alt; int min_alt; int min_alt5; int max_alt; int rela_max; int rela_ini; int flag1; int flag2; int flag3; unsigned char alttimes; void getAltitude(){ //wait_ms(300); Altitude a; Temperature t; Pressure p; APsensor.readAltitude(&a); APsensor.readTemperature(&t); APsensor.setModeStandby(); APsensor.setModeBarometer(); APsensor.setModeActive(); APsensor.readPressure(&p); alt = a; //気球試験用 if(phase == Preparing){ rela_ini = alt - ini_alt; if(rela_ini > 50)flag1 = flag1+1; } if(phase == Rising){ if(max_alt < alt){ max_alt = alt; } else{ rela_max = max_alt -alt; if(rela_max > 50)flag2 = flag2+1; } } //着地判定 if(phase == Descending){ if (min_alt > alt){ min_alt = alt; min_alt5 = min_alt + 5; flag3 -= 1; } else { if( alt < min_alt5){ flag3 +=2; } } } mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase); mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max); mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3); //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure()); APsensor.setModeStandby(); APsensor.setModeAltimeter(); APsensor.setModeActive(); } ///// //GPS int i,rlock,mode; char gps_data[256]; char ns,ew; float w_time,hokui,tokei; float g_hokui,g_tokei; float d_hokui,m_hokui,d_tokei,m_tokei; unsigned char c; //目標経度,緯度 float goal_tokei,goal_hokui; float goal_direction; float direction_y; float direction_x; float angular_difference; void getGPS(){ //目的地 ブラックロック修正 goal_tokei = 118.9833; goal_hokui = 40.92493; c = gps.getc(); if( c=='$' || i == 256){ mode = 0; i = 0; for(int j=0; j<256; j++){ gps_data[j]=NULL; } } if(mode==0){ if((gps_data[i]=c) != '\r'){ i++; }else{ gps_data[i]='\0'; if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ if(rlock==1){ //mp.printf("Status:Lock(%d)\n\r",rlock); //logitude d_tokei= int(tokei/100); m_tokei= (tokei-d_tokei*100)/60; g_tokei= d_tokei+m_tokei; //Latitude d_hokui=int(hokui/100); m_hokui=(hokui-d_hokui*100)/60; g_hokui=d_hokui+m_hokui; mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui); Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui); float longitudinalDifference =-(goal_tokei - g_tokei); float latitudinalDifference = goal_hokui - g_hokui; //球面三角法 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); 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); goal_direction = atan2f(direction_y,direction_x); if(goal_direction < 0)goal_direction += 2*M_PI; //磁北に対する角度に調節 ネバダ東に14度 goal_direction -= 0.244346; if (goal_direction < 0) goal_direction += 2*M_PI; mp.printf("goal:%f\n\r",goal_direction*180/M_PI); //mp.printf("longitudinalDifference:%f",longitudinalDifference); //mp.printf("latitudinalDifference:%f",latitudinalDifference); if(phase == Descending)getAltitude(); if(phase == Moving)step = attitude_determination; } else{ mp.printf("\n\rStatus:unLock(%d)\n\r",rlock); mp.printf("%s",gps_data); Mu2.printf("@DT02No\r\n"); if(phase == Descending)getAltitude(); //if(phase == Moving)test_get_direction(); } sprintf(gps_data, ""); } } } } void MU2initialize(){ Mu2.baud(19200); Mu2.printf("@EI34\r\n"); wait(1); Mu2.printf("@DI25\r\n"); wait(1); Mu2.printf("@CH0F\r\n"); wait(1); Mu2.printf("@GI34\r\n"); wait(1); } ///////////////////// //地磁気 HMC5883L compass(p9, p10); Ticker interrupt; double heading0 = 0.0; double heading1 = 0.0; double heading2 = 0.0; double heading3 = 0.0; double headingLPF = 0.0; double initHeading; double tgtHeading; double preHeading = 0.0; unsigned char hmc_mode; int maxX, minX, maxY, minY; int ofsX = 0; //calibration x int ofsY = 0; //calibration y int counter = 0; int ofsXset = 1122; int ofsYset = -466; int ofsZset = -522; //地磁気キャリブレーション用 void intrpt() { int16_t raw[3]; compass.getXYZ(raw); //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2] double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset)); if(heading < 0)heading += 2*M_PI; if(heading > 2*M_PI)heading -= 2*M_PI; heading3 = heading2; heading2 = heading1; heading1 = heading0; heading0 = heading; headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter switch(hmc_mode) { case STOP: if(counter == 100) { initHeading = headingLPF; hmc_mode = CAL; maxX = raw[0]; minX = raw[0]; maxY = raw[2]; minY = raw[2]; counter = 0; //mp.printf("STOP end\n\r"); } break; case CAL: if(raw[0] > maxX)maxX = raw[0]; if(raw[0] < minX)minX = raw[0]; if(raw[2] > maxY)maxY = raw[2]; if(raw[2] < minY)minY = raw[2]; if((counter > 100) && (headingLPF > (initHeading-0.01)) && (headingLPF < (initHeading+0.01))) { ofsX = (maxX + minX)/2; ofsY = (maxY + minY)/2; //hmc_mode = RUN; counter = 0; //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY); } break; case RUN: mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset); //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]); //step = calculate_direction; break; } counter++; } //加速度 MPU6050 mpu; int16_t ax, ay, az; int16_t gx, gy, gz; int32_t axa, aya, aza; int32_t gxa, gya, gza; //オフセット値 double axoffset = 586.711; double ayoffset = -174.847; double azoffset = -2195.34375; //センサの出力あたりの加速度[m/s^2] double axrate = 0.000601615; double ayrate = 0.000598334; double azrate = 0.0005845; //3軸キャリブレーション double phi; double theta; double heading3axis; double bfy; double bfx; //3軸からheading算出 void getheading3axis() { //加速度算出 mpu.getAcceleration(&ax, &ay, &az); double axcal = -axrate * (static_cast<double>(ax) - axoffset); double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); double azcal = -azrate * (static_cast<double>(az) - azoffset); phi = atan2(aycal,azcal); //phi += M_PI; theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi)); //theta += M_PI; //地磁気算出 int16_t raw[3]; compass.getXYZ(raw); bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi); bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta) + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi) + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi); double heading = atan2(-bfy, bfx); if(heading < 0)heading += 2*M_PI; if(heading > 2*M_PI)heading -= 2*M_PI; heading3 = heading2; heading2 = heading1; heading1 = heading0; heading0 = heading; headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter //headingLPF += 0.349066; if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI; //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal); mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI); } //ADXL375 const int addr = 0xA6; char adxl[6]; short int axout = 0; short int ayout = 0; short int azout = 0; void adxl_init(){ char fifo[2] = {0x38,0x80}; i2c.write(addr,fifo,2); //FIFO_Mode -> Stream char power[2] = {0x2D,0x08}; i2c.write(addr,power,2); //measurebit -> 1 } void adxl_get(){ adxl[0] = 0x32; i2c.write(addr,adxl,1); i2c.read(0xA7,adxl,6); axout = (((int16_t )adxl[1]) << 8) | adxl[0]; ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; azout = (((int16_t )adxl[5]) << 8) | adxl[4]; } //モーター駆動方向制御 void motor_drive(){ controlmotor1.write(1); controlmotor2.write(0); pin21.write(0.8); //if(pin1)mp.printf("Yes!\r\n"); } //溶断プログラム void burning(){ wait(5); myled1 = 1; para = 1; wait(1); myled1 = 0; para = 0; wait(5); myled2 = 1; marker = 1; wait(1); myled2 = 0; marker = 0; wait(5); myled3 = 1; jumparm = 1; wait(1); myled3 = 0; jumparm = 0; } //跳躍モーター駆動まいまいかわいい void jumping(){ timer.start(); //モーター駆動時間調整 while(val < 4){ val = timer.read(); jumpmotor = 1; myled4 != myled4; } timer.reset(); timer.stop(); jumpmotor = 0; val = 0; } ///test //GPSなし方向制御,跳躍テスト float test_tokei,test_hokui; void test_get_direction(){ //目的地 大岡山駅 goal_tokei = 139.686234; goal_hokui = 35.614330; //テスト場所石川台一号館 test_tokei = 139.681529; test_hokui = 35.605263; float longitudinalDifference = goal_tokei - test_tokei; float latitudinalDifference = goal_hokui - test_hokui; //球面三角法 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); 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); goal_direction = atan2f(direction_y,direction_x); if(goal_direction < 0)goal_direction += 2*M_PI; //磁北に対する角度に調節 東京7度 goal_direction += 0.122173; //goal_direction2 += 0.122173; if (goal_direction < 0) goal_direction += 2*M_PI; } /////////////////////////main//////////////////////////////////// int main(){ PHY_PowerDown();//省電力 wait(2); //センサ初期化 Mu2.baud(19200); Mu2.printf("@EI34\r\n"); wait(1); Mu2.printf("@DI25\r\n"); wait(1); Mu2.printf("@CH0F\r\n"); wait(1); Mu2.printf("@GI34\r\n"); wait(1); APsensor.init(); wait(0.2); hmc_mode = RUN; mpu.initialize(); wait(0.2); compass.init(); wait(0.2); adxl_init(); wait(0.2); APsensor.setOffsetAltitude(100); APsensor.setOffsetTemperature(10); APsensor.setOffsetPressure(-32); mkdir("/sd/mydir", 0777); min_alt = 4000.0; Flightpin = 1; //高度初期値 for(int h = 0; h < 10; h++){ getAltitude(); ini_alt = ini_alt + alt; wait(0.5); } ini_alt = ini_alt/10; mp.printf("ini_alt:%d\r\n",ini_alt); int jump_count; phase = Preparing; wait(900);//開始15分待機 //phase = Moving; /////////////////////////////単機能//////// //wait(5); //while(1){ //myled1 = 1; //Flightpin = 1; //GPS_MU2 //getGPS(); //方向制御用モーター /* controlmotor1.write(1); controlmotor2.write(0); pin21.write(0.8); wait(3); controlmotor1.write(0); controlmotor2.write(0); pin21.write(0.0); */ //跳躍用モーター //jumpmotor = 1; //wait(5); //jumpmotor = 0; //溶断 //burning(); //気圧 //getAltitude(); //mpu /* mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); double axcal = -axrate * (static_cast<double>(ax) - axoffset); double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); double azcal = -azrate * (static_cast<double>(az) - azoffset); mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal); */ //hmc /* hmc_mode = RUN; intrpt(); wait(0.5); */ //hmc_mpu //getheading3axis() ; //wait(0.3); //adxl_SDカード 跳躍高さ測定 /* mkdir("/sd/mydir", 0777); wait(1); FILE *fp = fopen("/sd/mydir/height1.txt", "a"); wait(1); if(fp == NULL) { error("Could not open file for write\n"); } timer.reset(); timer.start(); val = 0; while(val < 5000){ jumpmotor = 1; val = timer.read_ms(); adxl[0] = 0x32; i2c.write(addr,adxl,1); i2c.read(0xA7,adxl,6); axout = (((int16_t )adxl[1]) << 8) | adxl[0]; ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; azout = (((int16_t )adxl[5]) << 8) | adxl[4]; fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); wait_ms(2); myled1 !=myled1; } myled1 = 0; jumpmotor = 0; wait(1); fclose(fp); */ // //} ///////////////////////// /* //////////////////////////安全試験用///////// //振動試験 myled1 = 0; //wait(5);//test wait(900);//end to end //静荷重 FILE *sl = fopen("/local/static2.txt", "a"); wait(10); myled1 = 1; wait(5); timer.reset(); timer.start(); //while(val < 30000){ //test while(val < 600000){ //end to end val = timer.read_ms(); adxl[0] = 0x32; i2c.write(addr,adxl,1); i2c.read(0xA7,adxl,6); axout = (((int16_t )adxl[1]) << 8) | adxl[0]; ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; azout = (((int16_t )adxl[5]) << 8) | adxl[4]; //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout); fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); wait_ms(200); //wait必要、SDがbusyの状態で送っちゃってるんじゃない? } fclose(sl); timer.reset(); timer.stop(); val = 0; myled1 = 0; //wait(5); //test wait(960); //end to end //開傘衝撃 FILE *im = fopen("/local/impact2.txt", "a"); wait(10); myled2 = 1; timer.reset(); timer.start(); //while(val < 10000){ //test while(val < 30000){ //end to end val = timer.read_ms(); adxl[0] = 0x32; i2c.write(addr,adxl,1); i2c.read(0xA7,adxl,6); axout = (((int16_t )adxl[1]) << 8) | adxl[0]; ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; azout = (((int16_t )adxl[5]) << 8) | adxl[4]; fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); wait_ms(3); //wait必要、SDがbusyの状態で送っちゃってるんじゃない? } fclose(im); timer.reset(); timer.stop(); val = 0; myled2 = 0; //wait(30); //test wait(600); //end to end /////////////////////////// */ //着地検知用timer timer.reset(); timer.start(); val = 0; while(1){ switch(phase){ case Preparing: myled1 = 1; getAltitude(); wait(0.5); val = timer.read(); if(flag1 > 20) phase = Rising; if( val > 600) phase = Rising; //計25分 break; case Rising: myled1 = 0; myled2 = 1; getAltitude(); wait(0.5); val = timer.read(); if(flag2 > 10) phase = Descending; if(val > 900) phase = Descending; break; case Descending: val = timer.read(); myled2 = 0; myled3 = 1; getGPS(); if ( val >1500){ timer.reset(); val = 0; timer_set = 1; } if (timer_set == 1){ val_total = 1500 + val; } if ( flag3 >= 300){ if( val_total > 1800){ phase = Landing_Fusing; } } if( val_total > 2400) phase = Landing_Fusing;//電源onから55分 break; case Landing_Fusing: timer.reset(); timer.stop(); val = 0; myled3 = 0; myled4 = 1; wait(30); burning(); wait(5); Mu2.printf("@DT04FIRE\r\n"); for ( int gp; gp <= 10; gp++){ getGPS(); } wait(5); jumpmotor = 1; wait(15); jumpmotor = 0; phase = Moving; break; case Moving: myled2 = 0; myled1 = 0; myled3 = 0; myled4 = 0; wait(1); switch(step){ case get_goaldirection: myled1 = 1; getGPS(); //test_get_direction(); step = attitude_determination; wait(1); break; case attitude_determination: myled1 = 0; myled2 = 1; timer.reset(); timer.start(); val = 0; while(val < 3){ val = timer.read(); getheading3axis(); wait(0.5); } getheading3axis(); step = calculate_direction; wait(1); break; case calculate_direction: myled2 = 0; myled3 = 1; if(headingLPF < M_PI){ if(goal_direction < headingLPF+M_PI){ angular_difference = headingLPF - goal_direction; } else{ angular_difference = 2*M_PI + (headingLPF - goal_direction); } } else{ if(goal_direction < headingLPF - M_PI){ angular_difference = headingLPF - goal_direction - 2*M_PI; } else{ angular_difference = headingLPF - goal_direction; } } mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI); //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI); //40度以下でジャンプ if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){ step = jump; } else { step = direction_control; } wait(1); break; case direction_control: myled1 = 0; myled3 = 0; myled4 = 1; //int drive_time; timer.reset(); timer.stop(); timer.start(); val = 0; while(val < 2){ val = timer.read(); motor_drive(); } controlmotor1.write(0); controlmotor2.write(0); pin21.write(0.0); timer.reset(); timer.stop(); val = 0; //step = jump; if(jump_count == 3){ step = jump; } else{ step = attitude_determination; } jump_count = jump_count+1; break; case jump: //ログファイル myled1 = 1; myled2 = 1; wait(2); FILE *fp = fopen("/sd/mydir/height1.txt", "a"); wait(1); timer.reset(); timer.start(); val = 0; jumpmotor = 1; Mu2.printf("@DT04JUMP\r\n"); while(val < 4000){ val = timer.read_ms(); adxl[0] = 0x32; i2c.write(addr,adxl,1); i2c.read(0xA7,adxl,6); axout = (((int16_t )adxl[1]) << 8) | adxl[0]; ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; azout = (((int16_t )adxl[5]) << 8) | adxl[4]; fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); wait_ms(2); } jumpmotor = 0; wait(1); fclose(fp); myled3 = 1; jump_count = 0; wait(3); step = get_goaldirection; //step = direction_control; break; } } } }