k
Dependencies: Hm MPL MPU60580 MU2Class SDFileSystem mbed
main.cpp@1:8a25883c423c, 2017-09-13 (annotated)
- Committer:
- pyonta2017
- Date:
- Wed Sep 13 09:44:08 2017 +0000
- Revision:
- 1:8a25883c423c
- Parent:
- 0:6ddf1386e71d
aa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pyonta2017 | 0:6ddf1386e71d | 1 | #include "mbed.h" |
pyonta2017 | 0:6ddf1386e71d | 2 | #include "xprintf.h" |
pyonta2017 | 0:6ddf1386e71d | 3 | #include "MPL3115A2.h" |
pyonta2017 | 0:6ddf1386e71d | 4 | #include "HMC5883L.h" |
pyonta2017 | 0:6ddf1386e71d | 5 | #include "MPU6050.h" |
pyonta2017 | 0:6ddf1386e71d | 6 | #include "SDFileSystem.h" |
pyonta2017 | 0:6ddf1386e71d | 7 | #include "EthernetPowerControl.h" |
pyonta2017 | 0:6ddf1386e71d | 8 | #define M_PI 3.141592 |
pyonta2017 | 0:6ddf1386e71d | 9 | |
pyonta2017 | 0:6ddf1386e71d | 10 | //hmc calibration mode |
pyonta2017 | 0:6ddf1386e71d | 11 | #define STOP 0 //initial |
pyonta2017 | 0:6ddf1386e71d | 12 | #define CAL 1 //calibration |
pyonta2017 | 0:6ddf1386e71d | 13 | #define RUN 2 //run |
pyonta2017 | 0:6ddf1386e71d | 14 | |
pyonta2017 | 0:6ddf1386e71d | 15 | //Sequence phase |
pyonta2017 | 0:6ddf1386e71d | 16 | #define Preparing 1 |
pyonta2017 | 0:6ddf1386e71d | 17 | #define Rising 2 |
pyonta2017 | 0:6ddf1386e71d | 18 | #define Descending 3 |
pyonta2017 | 0:6ddf1386e71d | 19 | #define Landing_Fusing 4 |
pyonta2017 | 0:6ddf1386e71d | 20 | #define Moving 5 |
pyonta2017 | 0:6ddf1386e71d | 21 | |
pyonta2017 | 0:6ddf1386e71d | 22 | //Moving step |
pyonta2017 | 0:6ddf1386e71d | 23 | #define get_goaldirection 0 |
pyonta2017 | 0:6ddf1386e71d | 24 | #define attitude_determination 1 |
pyonta2017 | 0:6ddf1386e71d | 25 | #define calculate_direction 2 |
pyonta2017 | 0:6ddf1386e71d | 26 | #define direction_control 3 |
pyonta2017 | 0:6ddf1386e71d | 27 | #define jump 4 |
pyonta2017 | 0:6ddf1386e71d | 28 | |
pyonta2017 | 0:6ddf1386e71d | 29 | DigitalOut myled1(LED1); |
pyonta2017 | 0:6ddf1386e71d | 30 | DigitalOut myled2(LED2); |
pyonta2017 | 0:6ddf1386e71d | 31 | DigitalOut myled3(LED3); |
pyonta2017 | 0:6ddf1386e71d | 32 | DigitalOut myled4(LED4); |
pyonta2017 | 0:6ddf1386e71d | 33 | |
pyonta2017 | 0:6ddf1386e71d | 34 | //ピンアサイン確認 |
pyonta2017 | 0:6ddf1386e71d | 35 | Serial gps(p13, p14); |
pyonta2017 | 0:6ddf1386e71d | 36 | Serial Mu2(p28, p27); |
pyonta2017 | 0:6ddf1386e71d | 37 | I2C i2c(p9, p10); // sda, scl |
pyonta2017 | 0:6ddf1386e71d | 38 | Serial mp(USBTX, USBRX); |
pyonta2017 | 0:6ddf1386e71d | 39 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
pyonta2017 | 0:6ddf1386e71d | 40 | LocalFileSystem local("local"); |
pyonta2017 | 0:6ddf1386e71d | 41 | DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター |
pyonta2017 | 0:6ddf1386e71d | 42 | PwmOut pin21(p21); |
pyonta2017 | 0:6ddf1386e71d | 43 | DigitalOut jumpmotor(p24); |
pyonta2017 | 0:6ddf1386e71d | 44 | |
pyonta2017 | 0:6ddf1386e71d | 45 | //溶断 |
pyonta2017 | 0:6ddf1386e71d | 46 | DigitalOut para(p18); |
pyonta2017 | 0:6ddf1386e71d | 47 | DigitalOut marker(p19); |
pyonta2017 | 0:6ddf1386e71d | 48 | DigitalOut jumparm(p20); |
pyonta2017 | 0:6ddf1386e71d | 49 | // |
pyonta2017 | 0:6ddf1386e71d | 50 | DigitalOut Flightpin(p26); |
pyonta2017 | 0:6ddf1386e71d | 51 | |
pyonta2017 | 0:6ddf1386e71d | 52 | Timer timer; |
pyonta2017 | 0:6ddf1386e71d | 53 | int val; |
pyonta2017 | 0:6ddf1386e71d | 54 | int val_total; |
pyonta2017 | 0:6ddf1386e71d | 55 | unsigned char timer_set; |
pyonta2017 | 0:6ddf1386e71d | 56 | unsigned char phase; |
pyonta2017 | 0:6ddf1386e71d | 57 | unsigned char step; |
pyonta2017 | 1:8a25883c423c | 58 | unsigned char pitch; |
pyonta2017 | 0:6ddf1386e71d | 59 | |
pyonta2017 | 0:6ddf1386e71d | 60 | //気圧/////// |
pyonta2017 | 0:6ddf1386e71d | 61 | MPL3115A2 APsensor(&i2c, &mp); |
pyonta2017 | 0:6ddf1386e71d | 62 | int alt; |
pyonta2017 | 0:6ddf1386e71d | 63 | int ini_alt; |
pyonta2017 | 0:6ddf1386e71d | 64 | int min_alt; |
pyonta2017 | 0:6ddf1386e71d | 65 | int min_alt5; |
pyonta2017 | 0:6ddf1386e71d | 66 | int max_alt; |
pyonta2017 | 0:6ddf1386e71d | 67 | int rela_max; |
pyonta2017 | 0:6ddf1386e71d | 68 | int rela_ini; |
pyonta2017 | 0:6ddf1386e71d | 69 | int flag1; |
pyonta2017 | 0:6ddf1386e71d | 70 | int flag2; |
pyonta2017 | 0:6ddf1386e71d | 71 | int flag3; |
pyonta2017 | 0:6ddf1386e71d | 72 | unsigned char alttimes; |
pyonta2017 | 0:6ddf1386e71d | 73 | void getAltitude(){ |
pyonta2017 | 0:6ddf1386e71d | 74 | //wait_ms(300); |
pyonta2017 | 0:6ddf1386e71d | 75 | Altitude a; |
pyonta2017 | 0:6ddf1386e71d | 76 | Temperature t; |
pyonta2017 | 0:6ddf1386e71d | 77 | Pressure p; |
pyonta2017 | 0:6ddf1386e71d | 78 | APsensor.readAltitude(&a); |
pyonta2017 | 0:6ddf1386e71d | 79 | APsensor.readTemperature(&t); |
pyonta2017 | 0:6ddf1386e71d | 80 | APsensor.setModeStandby(); |
pyonta2017 | 0:6ddf1386e71d | 81 | APsensor.setModeBarometer(); |
pyonta2017 | 0:6ddf1386e71d | 82 | APsensor.setModeActive(); |
pyonta2017 | 0:6ddf1386e71d | 83 | APsensor.readPressure(&p); |
pyonta2017 | 0:6ddf1386e71d | 84 | alt = a; |
pyonta2017 | 0:6ddf1386e71d | 85 | //気球試験用 |
pyonta2017 | 0:6ddf1386e71d | 86 | if(phase == Preparing){ |
pyonta2017 | 0:6ddf1386e71d | 87 | rela_ini = alt - ini_alt; |
pyonta2017 | 0:6ddf1386e71d | 88 | if(rela_ini > 50)flag1 = flag1+1; |
pyonta2017 | 0:6ddf1386e71d | 89 | } |
pyonta2017 | 0:6ddf1386e71d | 90 | |
pyonta2017 | 0:6ddf1386e71d | 91 | if(phase == Rising){ |
pyonta2017 | 0:6ddf1386e71d | 92 | if(max_alt < alt){ |
pyonta2017 | 0:6ddf1386e71d | 93 | max_alt = alt; |
pyonta2017 | 0:6ddf1386e71d | 94 | } |
pyonta2017 | 0:6ddf1386e71d | 95 | else{ |
pyonta2017 | 0:6ddf1386e71d | 96 | rela_max = max_alt -alt; |
pyonta2017 | 1:8a25883c423c | 97 | if(rela_max > 150)flag2 = flag2+1; |
pyonta2017 | 0:6ddf1386e71d | 98 | } |
pyonta2017 | 0:6ddf1386e71d | 99 | } |
pyonta2017 | 0:6ddf1386e71d | 100 | //着地判定 |
pyonta2017 | 0:6ddf1386e71d | 101 | if(phase == Descending){ |
pyonta2017 | 0:6ddf1386e71d | 102 | if (min_alt > alt){ |
pyonta2017 | 0:6ddf1386e71d | 103 | min_alt = alt; |
pyonta2017 | 0:6ddf1386e71d | 104 | min_alt5 = min_alt + 5; |
pyonta2017 | 0:6ddf1386e71d | 105 | flag3 -= 1; |
pyonta2017 | 0:6ddf1386e71d | 106 | } |
pyonta2017 | 0:6ddf1386e71d | 107 | else { |
pyonta2017 | 0:6ddf1386e71d | 108 | if( alt < min_alt5){ |
pyonta2017 | 0:6ddf1386e71d | 109 | flag3 +=2; |
pyonta2017 | 0:6ddf1386e71d | 110 | } |
pyonta2017 | 0:6ddf1386e71d | 111 | } |
pyonta2017 | 0:6ddf1386e71d | 112 | } |
pyonta2017 | 0:6ddf1386e71d | 113 | mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase); |
pyonta2017 | 0:6ddf1386e71d | 114 | mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max); |
pyonta2017 | 0:6ddf1386e71d | 115 | mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3); |
pyonta2017 | 0:6ddf1386e71d | 116 | //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure()); |
pyonta2017 | 0:6ddf1386e71d | 117 | APsensor.setModeStandby(); |
pyonta2017 | 0:6ddf1386e71d | 118 | APsensor.setModeAltimeter(); |
pyonta2017 | 0:6ddf1386e71d | 119 | APsensor.setModeActive(); |
pyonta2017 | 0:6ddf1386e71d | 120 | } |
pyonta2017 | 0:6ddf1386e71d | 121 | ///// |
pyonta2017 | 0:6ddf1386e71d | 122 | |
pyonta2017 | 0:6ddf1386e71d | 123 | //GPS |
pyonta2017 | 0:6ddf1386e71d | 124 | int i,rlock,mode; |
pyonta2017 | 0:6ddf1386e71d | 125 | char gps_data[256]; |
pyonta2017 | 0:6ddf1386e71d | 126 | char ns,ew; |
pyonta2017 | 0:6ddf1386e71d | 127 | float w_time,hokui,tokei; |
pyonta2017 | 0:6ddf1386e71d | 128 | float g_hokui,g_tokei; |
pyonta2017 | 0:6ddf1386e71d | 129 | float d_hokui,m_hokui,d_tokei,m_tokei; |
pyonta2017 | 0:6ddf1386e71d | 130 | unsigned char c; |
pyonta2017 | 0:6ddf1386e71d | 131 | //目標経度,緯度 |
pyonta2017 | 0:6ddf1386e71d | 132 | float goal_tokei,goal_hokui; |
pyonta2017 | 0:6ddf1386e71d | 133 | float goal_direction; |
pyonta2017 | 0:6ddf1386e71d | 134 | float direction_y; |
pyonta2017 | 0:6ddf1386e71d | 135 | float direction_x; |
pyonta2017 | 0:6ddf1386e71d | 136 | float angular_difference; |
pyonta2017 | 0:6ddf1386e71d | 137 | |
pyonta2017 | 0:6ddf1386e71d | 138 | void getGPS(){ |
pyonta2017 | 0:6ddf1386e71d | 139 | //目的地 ブラックロック修正 |
pyonta2017 | 1:8a25883c423c | 140 | goal_tokei = 119.1218; |
pyonta2017 | 1:8a25883c423c | 141 | goal_hokui = 40.8797; |
pyonta2017 | 0:6ddf1386e71d | 142 | c = gps.getc(); |
pyonta2017 | 0:6ddf1386e71d | 143 | if( c=='$' || i == 256){ |
pyonta2017 | 0:6ddf1386e71d | 144 | mode = 0; |
pyonta2017 | 0:6ddf1386e71d | 145 | i = 0; |
pyonta2017 | 0:6ddf1386e71d | 146 | for(int j=0; j<256; j++){ |
pyonta2017 | 0:6ddf1386e71d | 147 | gps_data[j]=NULL; |
pyonta2017 | 0:6ddf1386e71d | 148 | } |
pyonta2017 | 0:6ddf1386e71d | 149 | } |
pyonta2017 | 0:6ddf1386e71d | 150 | if(mode==0){ |
pyonta2017 | 0:6ddf1386e71d | 151 | if((gps_data[i]=c) != '\r'){ |
pyonta2017 | 0:6ddf1386e71d | 152 | i++; |
pyonta2017 | 0:6ddf1386e71d | 153 | }else{ |
pyonta2017 | 0:6ddf1386e71d | 154 | gps_data[i]='\0'; |
pyonta2017 | 0:6ddf1386e71d | 155 | |
pyonta2017 | 0:6ddf1386e71d | 156 | if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ |
pyonta2017 | 0:6ddf1386e71d | 157 | if(rlock==1){ |
pyonta2017 | 0:6ddf1386e71d | 158 | //mp.printf("Status:Lock(%d)\n\r",rlock); |
pyonta2017 | 0:6ddf1386e71d | 159 | //logitude |
pyonta2017 | 0:6ddf1386e71d | 160 | d_tokei= int(tokei/100); |
pyonta2017 | 0:6ddf1386e71d | 161 | m_tokei= (tokei-d_tokei*100)/60; |
pyonta2017 | 0:6ddf1386e71d | 162 | g_tokei= d_tokei+m_tokei; |
pyonta2017 | 0:6ddf1386e71d | 163 | //Latitude |
pyonta2017 | 0:6ddf1386e71d | 164 | d_hokui=int(hokui/100); |
pyonta2017 | 0:6ddf1386e71d | 165 | m_hokui=(hokui-d_hokui*100)/60; |
pyonta2017 | 0:6ddf1386e71d | 166 | g_hokui=d_hokui+m_hokui; |
pyonta2017 | 0:6ddf1386e71d | 167 | mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui); |
pyonta2017 | 0:6ddf1386e71d | 168 | Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui); |
pyonta2017 | 0:6ddf1386e71d | 169 | float longitudinalDifference =-(goal_tokei - g_tokei); |
pyonta2017 | 0:6ddf1386e71d | 170 | float latitudinalDifference = goal_hokui - g_hokui; |
pyonta2017 | 0:6ddf1386e71d | 171 | |
pyonta2017 | 0:6ddf1386e71d | 172 | //球面三角法 |
pyonta2017 | 0:6ddf1386e71d | 173 | direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); |
pyonta2017 | 0:6ddf1386e71d | 174 | 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); |
pyonta2017 | 0:6ddf1386e71d | 175 | goal_direction = atan2f(direction_y,direction_x); |
pyonta2017 | 0:6ddf1386e71d | 176 | if(goal_direction < 0)goal_direction += 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 177 | |
pyonta2017 | 0:6ddf1386e71d | 178 | //磁北に対する角度に調節 ネバダ東に14度 |
pyonta2017 | 0:6ddf1386e71d | 179 | goal_direction -= 0.244346; |
pyonta2017 | 0:6ddf1386e71d | 180 | if (goal_direction < 0) goal_direction += 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 181 | mp.printf("goal:%f\n\r",goal_direction*180/M_PI); |
pyonta2017 | 0:6ddf1386e71d | 182 | //mp.printf("longitudinalDifference:%f",longitudinalDifference); |
pyonta2017 | 0:6ddf1386e71d | 183 | //mp.printf("latitudinalDifference:%f",latitudinalDifference); |
pyonta2017 | 0:6ddf1386e71d | 184 | if(phase == Descending)getAltitude(); |
pyonta2017 | 0:6ddf1386e71d | 185 | if(phase == Moving)step = attitude_determination; |
pyonta2017 | 0:6ddf1386e71d | 186 | } |
pyonta2017 | 0:6ddf1386e71d | 187 | else{ |
pyonta2017 | 0:6ddf1386e71d | 188 | mp.printf("\n\rStatus:unLock(%d)\n\r",rlock); |
pyonta2017 | 0:6ddf1386e71d | 189 | mp.printf("%s",gps_data); |
pyonta2017 | 0:6ddf1386e71d | 190 | Mu2.printf("@DT02No\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 191 | if(phase == Descending)getAltitude(); |
pyonta2017 | 1:8a25883c423c | 192 | if(phase == Moving)step = jump; |
pyonta2017 | 0:6ddf1386e71d | 193 | } |
pyonta2017 | 0:6ddf1386e71d | 194 | sprintf(gps_data, ""); |
pyonta2017 | 0:6ddf1386e71d | 195 | } |
pyonta2017 | 0:6ddf1386e71d | 196 | } |
pyonta2017 | 0:6ddf1386e71d | 197 | } |
pyonta2017 | 0:6ddf1386e71d | 198 | } |
pyonta2017 | 0:6ddf1386e71d | 199 | |
pyonta2017 | 0:6ddf1386e71d | 200 | void MU2initialize(){ |
pyonta2017 | 0:6ddf1386e71d | 201 | Mu2.baud(19200); |
pyonta2017 | 0:6ddf1386e71d | 202 | Mu2.printf("@EI34\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 203 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 204 | Mu2.printf("@DI25\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 205 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 206 | Mu2.printf("@CH0F\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 207 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 208 | Mu2.printf("@GI34\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 209 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 210 | } |
pyonta2017 | 0:6ddf1386e71d | 211 | |
pyonta2017 | 0:6ddf1386e71d | 212 | ///////////////////// |
pyonta2017 | 0:6ddf1386e71d | 213 | //地磁気 |
pyonta2017 | 0:6ddf1386e71d | 214 | HMC5883L compass(p9, p10); |
pyonta2017 | 0:6ddf1386e71d | 215 | Ticker interrupt; |
pyonta2017 | 0:6ddf1386e71d | 216 | double heading0 = 0.0; |
pyonta2017 | 0:6ddf1386e71d | 217 | double heading1 = 0.0; |
pyonta2017 | 0:6ddf1386e71d | 218 | double heading2 = 0.0; |
pyonta2017 | 0:6ddf1386e71d | 219 | double heading3 = 0.0; |
pyonta2017 | 0:6ddf1386e71d | 220 | double headingLPF = 0.0; |
pyonta2017 | 0:6ddf1386e71d | 221 | double initHeading; |
pyonta2017 | 0:6ddf1386e71d | 222 | double tgtHeading; |
pyonta2017 | 0:6ddf1386e71d | 223 | double preHeading = 0.0; |
pyonta2017 | 0:6ddf1386e71d | 224 | unsigned char hmc_mode; |
pyonta2017 | 0:6ddf1386e71d | 225 | |
pyonta2017 | 0:6ddf1386e71d | 226 | int maxX, minX, maxY, minY; |
pyonta2017 | 0:6ddf1386e71d | 227 | int ofsX = 0; //calibration x |
pyonta2017 | 0:6ddf1386e71d | 228 | int ofsY = 0; //calibration y |
pyonta2017 | 0:6ddf1386e71d | 229 | int counter = 0; |
pyonta2017 | 0:6ddf1386e71d | 230 | int ofsXset = 1122; |
pyonta2017 | 0:6ddf1386e71d | 231 | int ofsYset = -466; |
pyonta2017 | 0:6ddf1386e71d | 232 | int ofsZset = -522; |
pyonta2017 | 0:6ddf1386e71d | 233 | |
pyonta2017 | 0:6ddf1386e71d | 234 | //地磁気キャリブレーション用 |
pyonta2017 | 0:6ddf1386e71d | 235 | void intrpt() { |
pyonta2017 | 0:6ddf1386e71d | 236 | int16_t raw[3]; |
pyonta2017 | 0:6ddf1386e71d | 237 | compass.getXYZ(raw); |
pyonta2017 | 0:6ddf1386e71d | 238 | //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2] |
pyonta2017 | 0:6ddf1386e71d | 239 | double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset)); |
pyonta2017 | 0:6ddf1386e71d | 240 | if(heading < 0)heading += 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 241 | if(heading > 2*M_PI)heading -= 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 242 | heading3 = heading2; |
pyonta2017 | 0:6ddf1386e71d | 243 | heading2 = heading1; |
pyonta2017 | 0:6ddf1386e71d | 244 | heading1 = heading0; |
pyonta2017 | 0:6ddf1386e71d | 245 | heading0 = heading; |
pyonta2017 | 0:6ddf1386e71d | 246 | headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter |
pyonta2017 | 0:6ddf1386e71d | 247 | |
pyonta2017 | 0:6ddf1386e71d | 248 | switch(hmc_mode) { |
pyonta2017 | 0:6ddf1386e71d | 249 | case STOP: |
pyonta2017 | 0:6ddf1386e71d | 250 | if(counter == 100) { |
pyonta2017 | 0:6ddf1386e71d | 251 | initHeading = headingLPF; |
pyonta2017 | 0:6ddf1386e71d | 252 | hmc_mode = CAL; |
pyonta2017 | 0:6ddf1386e71d | 253 | maxX = raw[0]; |
pyonta2017 | 0:6ddf1386e71d | 254 | minX = raw[0]; |
pyonta2017 | 0:6ddf1386e71d | 255 | maxY = raw[2]; |
pyonta2017 | 0:6ddf1386e71d | 256 | minY = raw[2]; |
pyonta2017 | 0:6ddf1386e71d | 257 | counter = 0; |
pyonta2017 | 0:6ddf1386e71d | 258 | //mp.printf("STOP end\n\r"); |
pyonta2017 | 0:6ddf1386e71d | 259 | } |
pyonta2017 | 0:6ddf1386e71d | 260 | break; |
pyonta2017 | 0:6ddf1386e71d | 261 | case CAL: |
pyonta2017 | 0:6ddf1386e71d | 262 | if(raw[0] > maxX)maxX = raw[0]; |
pyonta2017 | 0:6ddf1386e71d | 263 | if(raw[0] < minX)minX = raw[0]; |
pyonta2017 | 0:6ddf1386e71d | 264 | if(raw[2] > maxY)maxY = raw[2]; |
pyonta2017 | 0:6ddf1386e71d | 265 | if(raw[2] < minY)minY = raw[2]; |
pyonta2017 | 0:6ddf1386e71d | 266 | if((counter > 100) |
pyonta2017 | 0:6ddf1386e71d | 267 | && (headingLPF > (initHeading-0.01)) |
pyonta2017 | 0:6ddf1386e71d | 268 | && (headingLPF < (initHeading+0.01))) { |
pyonta2017 | 0:6ddf1386e71d | 269 | ofsX = (maxX + minX)/2; |
pyonta2017 | 0:6ddf1386e71d | 270 | ofsY = (maxY + minY)/2; |
pyonta2017 | 0:6ddf1386e71d | 271 | //hmc_mode = RUN; |
pyonta2017 | 0:6ddf1386e71d | 272 | counter = 0; |
pyonta2017 | 0:6ddf1386e71d | 273 | //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY); |
pyonta2017 | 0:6ddf1386e71d | 274 | } |
pyonta2017 | 0:6ddf1386e71d | 275 | break; |
pyonta2017 | 0:6ddf1386e71d | 276 | case RUN: |
pyonta2017 | 0:6ddf1386e71d | 277 | mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset); |
pyonta2017 | 0:6ddf1386e71d | 278 | //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]); |
pyonta2017 | 0:6ddf1386e71d | 279 | //step = calculate_direction; |
pyonta2017 | 0:6ddf1386e71d | 280 | break; |
pyonta2017 | 0:6ddf1386e71d | 281 | |
pyonta2017 | 0:6ddf1386e71d | 282 | } |
pyonta2017 | 0:6ddf1386e71d | 283 | counter++; |
pyonta2017 | 0:6ddf1386e71d | 284 | } |
pyonta2017 | 0:6ddf1386e71d | 285 | |
pyonta2017 | 0:6ddf1386e71d | 286 | //加速度 |
pyonta2017 | 0:6ddf1386e71d | 287 | MPU6050 mpu; |
pyonta2017 | 0:6ddf1386e71d | 288 | int16_t ax, ay, az; |
pyonta2017 | 0:6ddf1386e71d | 289 | int16_t gx, gy, gz; |
pyonta2017 | 0:6ddf1386e71d | 290 | int32_t axa, aya, aza; |
pyonta2017 | 0:6ddf1386e71d | 291 | int32_t gxa, gya, gza; |
pyonta2017 | 0:6ddf1386e71d | 292 | |
pyonta2017 | 0:6ddf1386e71d | 293 | //オフセット値 |
pyonta2017 | 0:6ddf1386e71d | 294 | double axoffset = 586.711; |
pyonta2017 | 0:6ddf1386e71d | 295 | double ayoffset = -174.847; |
pyonta2017 | 0:6ddf1386e71d | 296 | double azoffset = -2195.34375; |
pyonta2017 | 0:6ddf1386e71d | 297 | |
pyonta2017 | 0:6ddf1386e71d | 298 | //センサの出力あたりの加速度[m/s^2] |
pyonta2017 | 0:6ddf1386e71d | 299 | double axrate = 0.000601615; |
pyonta2017 | 0:6ddf1386e71d | 300 | double ayrate = 0.000598334; |
pyonta2017 | 0:6ddf1386e71d | 301 | double azrate = 0.0005845; |
pyonta2017 | 0:6ddf1386e71d | 302 | |
pyonta2017 | 0:6ddf1386e71d | 303 | //3軸キャリブレーション |
pyonta2017 | 0:6ddf1386e71d | 304 | double phi; |
pyonta2017 | 0:6ddf1386e71d | 305 | double theta; |
pyonta2017 | 0:6ddf1386e71d | 306 | double heading3axis; |
pyonta2017 | 0:6ddf1386e71d | 307 | |
pyonta2017 | 0:6ddf1386e71d | 308 | double bfy; |
pyonta2017 | 0:6ddf1386e71d | 309 | double bfx; |
pyonta2017 | 0:6ddf1386e71d | 310 | |
pyonta2017 | 1:8a25883c423c | 311 | |
pyonta2017 | 0:6ddf1386e71d | 312 | //3軸からheading算出 |
pyonta2017 | 0:6ddf1386e71d | 313 | void getheading3axis() { |
pyonta2017 | 0:6ddf1386e71d | 314 | //加速度算出 |
pyonta2017 | 0:6ddf1386e71d | 315 | mpu.getAcceleration(&ax, &ay, &az); |
pyonta2017 | 0:6ddf1386e71d | 316 | double axcal = -axrate * (static_cast<double>(ax) - axoffset); |
pyonta2017 | 0:6ddf1386e71d | 317 | double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); |
pyonta2017 | 0:6ddf1386e71d | 318 | double azcal = -azrate * (static_cast<double>(az) - azoffset); |
pyonta2017 | 0:6ddf1386e71d | 319 | |
pyonta2017 | 0:6ddf1386e71d | 320 | phi = atan2(aycal,azcal); |
pyonta2017 | 0:6ddf1386e71d | 321 | //phi += M_PI; |
pyonta2017 | 0:6ddf1386e71d | 322 | theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi)); |
pyonta2017 | 1:8a25883c423c | 323 | if(theta > 0)pitch = 1; |
pyonta2017 | 1:8a25883c423c | 324 | if(theta < 0)pitch = 0; |
pyonta2017 | 0:6ddf1386e71d | 325 | //theta += M_PI; |
pyonta2017 | 0:6ddf1386e71d | 326 | |
pyonta2017 | 0:6ddf1386e71d | 327 | //地磁気算出 |
pyonta2017 | 0:6ddf1386e71d | 328 | int16_t raw[3]; |
pyonta2017 | 0:6ddf1386e71d | 329 | compass.getXYZ(raw); |
pyonta2017 | 0:6ddf1386e71d | 330 | bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi); |
pyonta2017 | 0:6ddf1386e71d | 331 | bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta) |
pyonta2017 | 0:6ddf1386e71d | 332 | + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi) |
pyonta2017 | 0:6ddf1386e71d | 333 | + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi); |
pyonta2017 | 0:6ddf1386e71d | 334 | double heading = atan2(-bfy, bfx); |
pyonta2017 | 0:6ddf1386e71d | 335 | if(heading < 0)heading += 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 336 | if(heading > 2*M_PI)heading -= 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 337 | heading3 = heading2; |
pyonta2017 | 0:6ddf1386e71d | 338 | heading2 = heading1; |
pyonta2017 | 0:6ddf1386e71d | 339 | heading1 = heading0; |
pyonta2017 | 0:6ddf1386e71d | 340 | heading0 = heading; |
pyonta2017 | 0:6ddf1386e71d | 341 | headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter |
pyonta2017 | 0:6ddf1386e71d | 342 | //headingLPF += 0.349066; |
pyonta2017 | 0:6ddf1386e71d | 343 | if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 344 | //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal); |
pyonta2017 | 0:6ddf1386e71d | 345 | mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI); |
pyonta2017 | 0:6ddf1386e71d | 346 | } |
pyonta2017 | 0:6ddf1386e71d | 347 | |
pyonta2017 | 0:6ddf1386e71d | 348 | //ADXL375 |
pyonta2017 | 0:6ddf1386e71d | 349 | const int addr = 0xA6; |
pyonta2017 | 0:6ddf1386e71d | 350 | char adxl[6]; |
pyonta2017 | 0:6ddf1386e71d | 351 | short int axout = 0; |
pyonta2017 | 0:6ddf1386e71d | 352 | short int ayout = 0; |
pyonta2017 | 0:6ddf1386e71d | 353 | short int azout = 0; |
pyonta2017 | 0:6ddf1386e71d | 354 | |
pyonta2017 | 0:6ddf1386e71d | 355 | void adxl_init(){ |
pyonta2017 | 0:6ddf1386e71d | 356 | char fifo[2] = {0x38,0x80}; |
pyonta2017 | 0:6ddf1386e71d | 357 | i2c.write(addr,fifo,2); //FIFO_Mode -> Stream |
pyonta2017 | 0:6ddf1386e71d | 358 | char power[2] = {0x2D,0x08}; |
pyonta2017 | 0:6ddf1386e71d | 359 | i2c.write(addr,power,2); //measurebit -> 1 |
pyonta2017 | 0:6ddf1386e71d | 360 | } |
pyonta2017 | 0:6ddf1386e71d | 361 | |
pyonta2017 | 0:6ddf1386e71d | 362 | void adxl_get(){ |
pyonta2017 | 0:6ddf1386e71d | 363 | adxl[0] = 0x32; |
pyonta2017 | 0:6ddf1386e71d | 364 | i2c.write(addr,adxl,1); |
pyonta2017 | 0:6ddf1386e71d | 365 | i2c.read(0xA7,adxl,6); |
pyonta2017 | 0:6ddf1386e71d | 366 | axout = (((int16_t )adxl[1]) << 8) | adxl[0]; |
pyonta2017 | 0:6ddf1386e71d | 367 | ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; |
pyonta2017 | 0:6ddf1386e71d | 368 | azout = (((int16_t )adxl[5]) << 8) | adxl[4]; |
pyonta2017 | 0:6ddf1386e71d | 369 | } |
pyonta2017 | 0:6ddf1386e71d | 370 | |
pyonta2017 | 0:6ddf1386e71d | 371 | |
pyonta2017 | 0:6ddf1386e71d | 372 | //モーター駆動方向制御 |
pyonta2017 | 0:6ddf1386e71d | 373 | void motor_drive(){ |
pyonta2017 | 0:6ddf1386e71d | 374 | controlmotor1.write(1); |
pyonta2017 | 0:6ddf1386e71d | 375 | controlmotor2.write(0); |
pyonta2017 | 1:8a25883c423c | 376 | pin21.write(1.0); |
pyonta2017 | 0:6ddf1386e71d | 377 | //if(pin1)mp.printf("Yes!\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 378 | } |
pyonta2017 | 0:6ddf1386e71d | 379 | |
pyonta2017 | 0:6ddf1386e71d | 380 | //溶断プログラム |
pyonta2017 | 1:8a25883c423c | 381 | //最終チェックポイント//////////////////////////////// |
pyonta2017 | 0:6ddf1386e71d | 382 | void burning(){ |
pyonta2017 | 0:6ddf1386e71d | 383 | wait(5); |
pyonta2017 | 0:6ddf1386e71d | 384 | myled1 = 1; |
pyonta2017 | 1:8a25883c423c | 385 | //para = 1; |
pyonta2017 | 1:8a25883c423c | 386 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 387 | myled1 = 0; |
pyonta2017 | 1:8a25883c423c | 388 | //para = 0; |
pyonta2017 | 0:6ddf1386e71d | 389 | wait(5); |
pyonta2017 | 0:6ddf1386e71d | 390 | myled2 = 1; |
pyonta2017 | 1:8a25883c423c | 391 | //marker = 1; |
pyonta2017 | 0:6ddf1386e71d | 392 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 393 | myled2 = 0; |
pyonta2017 | 1:8a25883c423c | 394 | //marker = 0; |
pyonta2017 | 0:6ddf1386e71d | 395 | wait(5); |
pyonta2017 | 0:6ddf1386e71d | 396 | myled3 = 1; |
pyonta2017 | 1:8a25883c423c | 397 | //jumparm = 1; |
pyonta2017 | 0:6ddf1386e71d | 398 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 399 | myled3 = 0; |
pyonta2017 | 1:8a25883c423c | 400 | //jumparm = 0; |
pyonta2017 | 0:6ddf1386e71d | 401 | } |
pyonta2017 | 0:6ddf1386e71d | 402 | //跳躍モーター駆動まいまいかわいい |
pyonta2017 | 0:6ddf1386e71d | 403 | void jumping(){ |
pyonta2017 | 0:6ddf1386e71d | 404 | timer.start(); |
pyonta2017 | 0:6ddf1386e71d | 405 | //モーター駆動時間調整 |
pyonta2017 | 0:6ddf1386e71d | 406 | while(val < 4){ |
pyonta2017 | 0:6ddf1386e71d | 407 | val = timer.read(); |
pyonta2017 | 0:6ddf1386e71d | 408 | jumpmotor = 1; |
pyonta2017 | 0:6ddf1386e71d | 409 | myled4 != myled4; |
pyonta2017 | 0:6ddf1386e71d | 410 | } |
pyonta2017 | 0:6ddf1386e71d | 411 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 412 | timer.stop(); |
pyonta2017 | 0:6ddf1386e71d | 413 | jumpmotor = 0; |
pyonta2017 | 0:6ddf1386e71d | 414 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 415 | } |
pyonta2017 | 0:6ddf1386e71d | 416 | |
pyonta2017 | 0:6ddf1386e71d | 417 | ///test |
pyonta2017 | 0:6ddf1386e71d | 418 | //GPSなし方向制御,跳躍テスト |
pyonta2017 | 0:6ddf1386e71d | 419 | float test_tokei,test_hokui; |
pyonta2017 | 0:6ddf1386e71d | 420 | void test_get_direction(){ |
pyonta2017 | 0:6ddf1386e71d | 421 | //目的地 大岡山駅 |
pyonta2017 | 0:6ddf1386e71d | 422 | goal_tokei = 139.686234; |
pyonta2017 | 0:6ddf1386e71d | 423 | goal_hokui = 35.614330; |
pyonta2017 | 0:6ddf1386e71d | 424 | |
pyonta2017 | 0:6ddf1386e71d | 425 | //テスト場所石川台一号館 |
pyonta2017 | 0:6ddf1386e71d | 426 | test_tokei = 139.681529; |
pyonta2017 | 0:6ddf1386e71d | 427 | test_hokui = 35.605263; |
pyonta2017 | 0:6ddf1386e71d | 428 | |
pyonta2017 | 0:6ddf1386e71d | 429 | float longitudinalDifference = goal_tokei - test_tokei; |
pyonta2017 | 0:6ddf1386e71d | 430 | float latitudinalDifference = goal_hokui - test_hokui; |
pyonta2017 | 0:6ddf1386e71d | 431 | //球面三角法 |
pyonta2017 | 0:6ddf1386e71d | 432 | direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180); |
pyonta2017 | 0:6ddf1386e71d | 433 | 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); |
pyonta2017 | 0:6ddf1386e71d | 434 | goal_direction = atan2f(direction_y,direction_x); |
pyonta2017 | 0:6ddf1386e71d | 435 | if(goal_direction < 0)goal_direction += 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 436 | |
pyonta2017 | 0:6ddf1386e71d | 437 | //磁北に対する角度に調節 東京7度 |
pyonta2017 | 0:6ddf1386e71d | 438 | goal_direction += 0.122173; |
pyonta2017 | 0:6ddf1386e71d | 439 | //goal_direction2 += 0.122173; |
pyonta2017 | 0:6ddf1386e71d | 440 | if (goal_direction < 0) goal_direction += 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 441 | } |
pyonta2017 | 0:6ddf1386e71d | 442 | |
pyonta2017 | 0:6ddf1386e71d | 443 | /////////////////////////main//////////////////////////////////// |
pyonta2017 | 0:6ddf1386e71d | 444 | int main(){ |
pyonta2017 | 0:6ddf1386e71d | 445 | PHY_PowerDown();//省電力 |
pyonta2017 | 0:6ddf1386e71d | 446 | wait(2); |
pyonta2017 | 0:6ddf1386e71d | 447 | //センサ初期化 |
pyonta2017 | 0:6ddf1386e71d | 448 | Mu2.baud(19200); |
pyonta2017 | 0:6ddf1386e71d | 449 | Mu2.printf("@EI34\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 450 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 451 | Mu2.printf("@DI25\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 452 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 453 | Mu2.printf("@CH0F\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 454 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 455 | Mu2.printf("@GI34\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 456 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 457 | APsensor.init(); |
pyonta2017 | 0:6ddf1386e71d | 458 | wait(0.2); |
pyonta2017 | 0:6ddf1386e71d | 459 | hmc_mode = RUN; |
pyonta2017 | 0:6ddf1386e71d | 460 | mpu.initialize(); |
pyonta2017 | 0:6ddf1386e71d | 461 | wait(0.2); |
pyonta2017 | 0:6ddf1386e71d | 462 | compass.init(); |
pyonta2017 | 0:6ddf1386e71d | 463 | wait(0.2); |
pyonta2017 | 0:6ddf1386e71d | 464 | adxl_init(); |
pyonta2017 | 0:6ddf1386e71d | 465 | wait(0.2); |
pyonta2017 | 0:6ddf1386e71d | 466 | APsensor.setOffsetAltitude(100); |
pyonta2017 | 0:6ddf1386e71d | 467 | APsensor.setOffsetTemperature(10); |
pyonta2017 | 0:6ddf1386e71d | 468 | APsensor.setOffsetPressure(-32); |
pyonta2017 | 0:6ddf1386e71d | 469 | mkdir("/sd/mydir", 0777); |
pyonta2017 | 1:8a25883c423c | 470 | min_alt = 5000.0; |
pyonta2017 | 0:6ddf1386e71d | 471 | Flightpin = 1; |
pyonta2017 | 0:6ddf1386e71d | 472 | //高度初期値 |
pyonta2017 | 0:6ddf1386e71d | 473 | for(int h = 0; h < 10; h++){ |
pyonta2017 | 0:6ddf1386e71d | 474 | getAltitude(); |
pyonta2017 | 0:6ddf1386e71d | 475 | ini_alt = ini_alt + alt; |
pyonta2017 | 0:6ddf1386e71d | 476 | wait(0.5); |
pyonta2017 | 0:6ddf1386e71d | 477 | } |
pyonta2017 | 0:6ddf1386e71d | 478 | ini_alt = ini_alt/10; |
pyonta2017 | 0:6ddf1386e71d | 479 | mp.printf("ini_alt:%d\r\n",ini_alt); |
pyonta2017 | 0:6ddf1386e71d | 480 | int jump_count; |
pyonta2017 | 1:8a25883c423c | 481 | |
pyonta2017 | 1:8a25883c423c | 482 | //最終チェックポイント///////////////////////////////////////// |
pyonta2017 | 1:8a25883c423c | 483 | //phase = Preparing; |
pyonta2017 | 1:8a25883c423c | 484 | //wait(1500);//開始25分待機 |
pyonta2017 | 1:8a25883c423c | 485 | //phase = Descending; |
pyonta2017 | 0:6ddf1386e71d | 486 | //phase = Moving; |
pyonta2017 | 1:8a25883c423c | 487 | phase = Landing_Fusing; |
pyonta2017 | 0:6ddf1386e71d | 488 | /////////////////////////////単機能//////// |
pyonta2017 | 0:6ddf1386e71d | 489 | |
pyonta2017 | 0:6ddf1386e71d | 490 | //wait(5); |
pyonta2017 | 0:6ddf1386e71d | 491 | |
pyonta2017 | 0:6ddf1386e71d | 492 | //while(1){ |
pyonta2017 | 0:6ddf1386e71d | 493 | //myled1 = 1; |
pyonta2017 | 0:6ddf1386e71d | 494 | //Flightpin = 1; |
pyonta2017 | 0:6ddf1386e71d | 495 | //GPS_MU2 |
pyonta2017 | 0:6ddf1386e71d | 496 | //getGPS(); |
pyonta2017 | 0:6ddf1386e71d | 497 | //方向制御用モーター |
pyonta2017 | 0:6ddf1386e71d | 498 | /* |
pyonta2017 | 0:6ddf1386e71d | 499 | controlmotor1.write(1); |
pyonta2017 | 0:6ddf1386e71d | 500 | controlmotor2.write(0); |
pyonta2017 | 0:6ddf1386e71d | 501 | pin21.write(0.8); |
pyonta2017 | 0:6ddf1386e71d | 502 | wait(3); |
pyonta2017 | 0:6ddf1386e71d | 503 | controlmotor1.write(0); |
pyonta2017 | 0:6ddf1386e71d | 504 | controlmotor2.write(0); |
pyonta2017 | 0:6ddf1386e71d | 505 | pin21.write(0.0); |
pyonta2017 | 0:6ddf1386e71d | 506 | */ |
pyonta2017 | 0:6ddf1386e71d | 507 | //跳躍用モーター |
pyonta2017 | 0:6ddf1386e71d | 508 | //jumpmotor = 1; |
pyonta2017 | 0:6ddf1386e71d | 509 | //wait(5); |
pyonta2017 | 0:6ddf1386e71d | 510 | //jumpmotor = 0; |
pyonta2017 | 0:6ddf1386e71d | 511 | //溶断 |
pyonta2017 | 0:6ddf1386e71d | 512 | //burning(); |
pyonta2017 | 0:6ddf1386e71d | 513 | //気圧 |
pyonta2017 | 0:6ddf1386e71d | 514 | //getAltitude(); |
pyonta2017 | 0:6ddf1386e71d | 515 | //mpu |
pyonta2017 | 0:6ddf1386e71d | 516 | /* |
pyonta2017 | 0:6ddf1386e71d | 517 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
pyonta2017 | 0:6ddf1386e71d | 518 | double axcal = -axrate * (static_cast<double>(ax) - axoffset); |
pyonta2017 | 0:6ddf1386e71d | 519 | double aycal = -ayrate * (static_cast<double>(ay) - ayoffset); |
pyonta2017 | 0:6ddf1386e71d | 520 | double azcal = -azrate * (static_cast<double>(az) - azoffset); |
pyonta2017 | 0:6ddf1386e71d | 521 | mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal); |
pyonta2017 | 0:6ddf1386e71d | 522 | */ |
pyonta2017 | 0:6ddf1386e71d | 523 | //hmc |
pyonta2017 | 0:6ddf1386e71d | 524 | /* |
pyonta2017 | 0:6ddf1386e71d | 525 | hmc_mode = RUN; |
pyonta2017 | 0:6ddf1386e71d | 526 | intrpt(); |
pyonta2017 | 0:6ddf1386e71d | 527 | wait(0.5); |
pyonta2017 | 0:6ddf1386e71d | 528 | */ |
pyonta2017 | 0:6ddf1386e71d | 529 | //hmc_mpu |
pyonta2017 | 0:6ddf1386e71d | 530 | //getheading3axis() ; |
pyonta2017 | 0:6ddf1386e71d | 531 | //wait(0.3); |
pyonta2017 | 0:6ddf1386e71d | 532 | //adxl_SDカード 跳躍高さ測定 |
pyonta2017 | 0:6ddf1386e71d | 533 | /* |
pyonta2017 | 0:6ddf1386e71d | 534 | mkdir("/sd/mydir", 0777); |
pyonta2017 | 0:6ddf1386e71d | 535 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 536 | FILE *fp = fopen("/sd/mydir/height1.txt", "a"); |
pyonta2017 | 0:6ddf1386e71d | 537 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 538 | if(fp == NULL) { |
pyonta2017 | 0:6ddf1386e71d | 539 | error("Could not open file for write\n"); |
pyonta2017 | 0:6ddf1386e71d | 540 | } |
pyonta2017 | 0:6ddf1386e71d | 541 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 542 | timer.start(); |
pyonta2017 | 0:6ddf1386e71d | 543 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 544 | while(val < 5000){ |
pyonta2017 | 0:6ddf1386e71d | 545 | jumpmotor = 1; |
pyonta2017 | 0:6ddf1386e71d | 546 | val = timer.read_ms(); |
pyonta2017 | 0:6ddf1386e71d | 547 | adxl[0] = 0x32; |
pyonta2017 | 0:6ddf1386e71d | 548 | i2c.write(addr,adxl,1); |
pyonta2017 | 0:6ddf1386e71d | 549 | i2c.read(0xA7,adxl,6); |
pyonta2017 | 0:6ddf1386e71d | 550 | axout = (((int16_t )adxl[1]) << 8) | adxl[0]; |
pyonta2017 | 0:6ddf1386e71d | 551 | ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; |
pyonta2017 | 0:6ddf1386e71d | 552 | azout = (((int16_t )adxl[5]) << 8) | adxl[4]; |
pyonta2017 | 0:6ddf1386e71d | 553 | fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); |
pyonta2017 | 0:6ddf1386e71d | 554 | wait_ms(2); |
pyonta2017 | 0:6ddf1386e71d | 555 | myled1 !=myled1; |
pyonta2017 | 0:6ddf1386e71d | 556 | } |
pyonta2017 | 0:6ddf1386e71d | 557 | myled1 = 0; |
pyonta2017 | 0:6ddf1386e71d | 558 | jumpmotor = 0; |
pyonta2017 | 0:6ddf1386e71d | 559 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 560 | fclose(fp); |
pyonta2017 | 0:6ddf1386e71d | 561 | */ |
pyonta2017 | 0:6ddf1386e71d | 562 | // |
pyonta2017 | 0:6ddf1386e71d | 563 | |
pyonta2017 | 0:6ddf1386e71d | 564 | //} |
pyonta2017 | 0:6ddf1386e71d | 565 | ///////////////////////// |
pyonta2017 | 1:8a25883c423c | 566 | |
pyonta2017 | 0:6ddf1386e71d | 567 | //着地検知用timer |
pyonta2017 | 0:6ddf1386e71d | 568 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 569 | timer.start(); |
pyonta2017 | 0:6ddf1386e71d | 570 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 571 | while(1){ |
pyonta2017 | 0:6ddf1386e71d | 572 | switch(phase){ |
pyonta2017 | 0:6ddf1386e71d | 573 | case Preparing: |
pyonta2017 | 0:6ddf1386e71d | 574 | myled1 = 1; |
pyonta2017 | 0:6ddf1386e71d | 575 | getAltitude(); |
pyonta2017 | 0:6ddf1386e71d | 576 | wait(0.5); |
pyonta2017 | 0:6ddf1386e71d | 577 | val = timer.read(); |
pyonta2017 | 0:6ddf1386e71d | 578 | if(flag1 > 20) phase = Rising; |
pyonta2017 | 1:8a25883c423c | 579 | if( val > 600) phase = Rising; //計35分 |
pyonta2017 | 0:6ddf1386e71d | 580 | break; |
pyonta2017 | 0:6ddf1386e71d | 581 | case Rising: |
pyonta2017 | 0:6ddf1386e71d | 582 | myled1 = 0; |
pyonta2017 | 0:6ddf1386e71d | 583 | myled2 = 1; |
pyonta2017 | 0:6ddf1386e71d | 584 | getAltitude(); |
pyonta2017 | 0:6ddf1386e71d | 585 | wait(0.5); |
pyonta2017 | 0:6ddf1386e71d | 586 | val = timer.read(); |
pyonta2017 | 1:8a25883c423c | 587 | if(flag2 > 40) phase = Descending; |
pyonta2017 | 1:8a25883c423c | 588 | if(val > 1800) phase = Descending; //計55分 |
pyonta2017 | 0:6ddf1386e71d | 589 | break; |
pyonta2017 | 0:6ddf1386e71d | 590 | case Descending: |
pyonta2017 | 0:6ddf1386e71d | 591 | val = timer.read(); |
pyonta2017 | 0:6ddf1386e71d | 592 | myled2 = 0; |
pyonta2017 | 0:6ddf1386e71d | 593 | myled3 = 1; |
pyonta2017 | 1:8a25883c423c | 594 | if(flag3 < 50){ |
pyonta2017 | 1:8a25883c423c | 595 | getGPS(); |
pyonta2017 | 1:8a25883c423c | 596 | } |
pyonta2017 | 1:8a25883c423c | 597 | else{ |
pyonta2017 | 1:8a25883c423c | 598 | getAltitude(); |
pyonta2017 | 1:8a25883c423c | 599 | } |
pyonta2017 | 0:6ddf1386e71d | 600 | if ( val >1500){ |
pyonta2017 | 0:6ddf1386e71d | 601 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 602 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 603 | timer_set = 1; |
pyonta2017 | 0:6ddf1386e71d | 604 | } |
pyonta2017 | 0:6ddf1386e71d | 605 | if (timer_set == 1){ |
pyonta2017 | 0:6ddf1386e71d | 606 | val_total = 1500 + val; |
pyonta2017 | 1:8a25883c423c | 607 | } |
pyonta2017 | 1:8a25883c423c | 608 | //最終チェックポイント////////////////////////////////////////// |
pyonta2017 | 1:8a25883c423c | 609 | if ( flag3 >= 200){ |
pyonta2017 | 1:8a25883c423c | 610 | if( val > 900){ |
pyonta2017 | 0:6ddf1386e71d | 611 | phase = Landing_Fusing; |
pyonta2017 | 0:6ddf1386e71d | 612 | } |
pyonta2017 | 0:6ddf1386e71d | 613 | } |
pyonta2017 | 1:8a25883c423c | 614 | if( val_total >2400 ){ |
pyonta2017 | 1:8a25883c423c | 615 | wait(600); |
pyonta2017 | 1:8a25883c423c | 616 | phase = Landing_Fusing;//電源onから75分 |
pyonta2017 | 1:8a25883c423c | 617 | } |
pyonta2017 | 0:6ddf1386e71d | 618 | break; |
pyonta2017 | 0:6ddf1386e71d | 619 | case Landing_Fusing: |
pyonta2017 | 0:6ddf1386e71d | 620 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 621 | timer.stop(); |
pyonta2017 | 0:6ddf1386e71d | 622 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 623 | myled3 = 0; |
pyonta2017 | 0:6ddf1386e71d | 624 | myled4 = 1; |
pyonta2017 | 1:8a25883c423c | 625 | //wait(30); |
pyonta2017 | 0:6ddf1386e71d | 626 | burning(); |
pyonta2017 | 0:6ddf1386e71d | 627 | wait(5); |
pyonta2017 | 0:6ddf1386e71d | 628 | Mu2.printf("@DT04FIRE\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 629 | for ( int gp; gp <= 10; gp++){ |
pyonta2017 | 0:6ddf1386e71d | 630 | getGPS(); |
pyonta2017 | 0:6ddf1386e71d | 631 | } |
pyonta2017 | 1:8a25883c423c | 632 | |
pyonta2017 | 1:8a25883c423c | 633 | wait(2); |
pyonta2017 | 0:6ddf1386e71d | 634 | jumpmotor = 1; |
pyonta2017 | 1:8a25883c423c | 635 | wait(10); |
pyonta2017 | 1:8a25883c423c | 636 | jumpmotor = 0; |
pyonta2017 | 1:8a25883c423c | 637 | wait(2); |
pyonta2017 | 1:8a25883c423c | 638 | pin21.write(1.0); |
pyonta2017 | 1:8a25883c423c | 639 | controlmotor1.write(0); |
pyonta2017 | 1:8a25883c423c | 640 | controlmotor2.write(1); |
pyonta2017 | 1:8a25883c423c | 641 | wait(4); |
pyonta2017 | 1:8a25883c423c | 642 | pin21.write(0.0); |
pyonta2017 | 1:8a25883c423c | 643 | controlmotor1.write(0); |
pyonta2017 | 1:8a25883c423c | 644 | controlmotor2.write(0); |
pyonta2017 | 1:8a25883c423c | 645 | wait(3); |
pyonta2017 | 1:8a25883c423c | 646 | jumpmotor = 1; |
pyonta2017 | 1:8a25883c423c | 647 | wait(10); |
pyonta2017 | 0:6ddf1386e71d | 648 | jumpmotor = 0; |
pyonta2017 | 0:6ddf1386e71d | 649 | phase = Moving; |
pyonta2017 | 0:6ddf1386e71d | 650 | break; |
pyonta2017 | 0:6ddf1386e71d | 651 | case Moving: |
pyonta2017 | 0:6ddf1386e71d | 652 | myled2 = 0; |
pyonta2017 | 0:6ddf1386e71d | 653 | myled1 = 0; |
pyonta2017 | 0:6ddf1386e71d | 654 | myled3 = 0; |
pyonta2017 | 0:6ddf1386e71d | 655 | myled4 = 0; |
pyonta2017 | 0:6ddf1386e71d | 656 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 657 | switch(step){ |
pyonta2017 | 0:6ddf1386e71d | 658 | case get_goaldirection: |
pyonta2017 | 0:6ddf1386e71d | 659 | myled1 = 1; |
pyonta2017 | 0:6ddf1386e71d | 660 | getGPS(); |
pyonta2017 | 0:6ddf1386e71d | 661 | step = attitude_determination; |
pyonta2017 | 0:6ddf1386e71d | 662 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 663 | break; |
pyonta2017 | 0:6ddf1386e71d | 664 | case attitude_determination: |
pyonta2017 | 0:6ddf1386e71d | 665 | myled1 = 0; |
pyonta2017 | 0:6ddf1386e71d | 666 | myled2 = 1; |
pyonta2017 | 0:6ddf1386e71d | 667 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 668 | timer.start(); |
pyonta2017 | 0:6ddf1386e71d | 669 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 670 | while(val < 3){ |
pyonta2017 | 0:6ddf1386e71d | 671 | val = timer.read(); |
pyonta2017 | 0:6ddf1386e71d | 672 | getheading3axis(); |
pyonta2017 | 0:6ddf1386e71d | 673 | wait(0.5); |
pyonta2017 | 0:6ddf1386e71d | 674 | } |
pyonta2017 | 1:8a25883c423c | 675 | timer.reset(); |
pyonta2017 | 1:8a25883c423c | 676 | timer.stop(); |
pyonta2017 | 1:8a25883c423c | 677 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 678 | getheading3axis(); |
pyonta2017 | 0:6ddf1386e71d | 679 | step = calculate_direction; |
pyonta2017 | 0:6ddf1386e71d | 680 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 681 | break; |
pyonta2017 | 0:6ddf1386e71d | 682 | case calculate_direction: |
pyonta2017 | 0:6ddf1386e71d | 683 | myled2 = 0; |
pyonta2017 | 0:6ddf1386e71d | 684 | myled3 = 1; |
pyonta2017 | 0:6ddf1386e71d | 685 | if(headingLPF < M_PI){ |
pyonta2017 | 0:6ddf1386e71d | 686 | if(goal_direction < headingLPF+M_PI){ |
pyonta2017 | 0:6ddf1386e71d | 687 | angular_difference = headingLPF - goal_direction; |
pyonta2017 | 0:6ddf1386e71d | 688 | } |
pyonta2017 | 0:6ddf1386e71d | 689 | else{ |
pyonta2017 | 0:6ddf1386e71d | 690 | angular_difference = 2*M_PI + (headingLPF - goal_direction); |
pyonta2017 | 0:6ddf1386e71d | 691 | } |
pyonta2017 | 0:6ddf1386e71d | 692 | } |
pyonta2017 | 0:6ddf1386e71d | 693 | else{ |
pyonta2017 | 0:6ddf1386e71d | 694 | if(goal_direction < headingLPF - M_PI){ |
pyonta2017 | 0:6ddf1386e71d | 695 | angular_difference = headingLPF - goal_direction - 2*M_PI; |
pyonta2017 | 0:6ddf1386e71d | 696 | } |
pyonta2017 | 0:6ddf1386e71d | 697 | else{ |
pyonta2017 | 0:6ddf1386e71d | 698 | angular_difference = headingLPF - goal_direction; |
pyonta2017 | 0:6ddf1386e71d | 699 | } |
pyonta2017 | 0:6ddf1386e71d | 700 | } |
pyonta2017 | 0:6ddf1386e71d | 701 | mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI); |
pyonta2017 | 0:6ddf1386e71d | 702 | //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI); |
pyonta2017 | 0:6ddf1386e71d | 703 | //40度以下でジャンプ |
pyonta2017 | 0:6ddf1386e71d | 704 | if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){ |
pyonta2017 | 0:6ddf1386e71d | 705 | step = jump; |
pyonta2017 | 0:6ddf1386e71d | 706 | } |
pyonta2017 | 0:6ddf1386e71d | 707 | else { |
pyonta2017 | 0:6ddf1386e71d | 708 | step = direction_control; |
pyonta2017 | 0:6ddf1386e71d | 709 | } |
pyonta2017 | 1:8a25883c423c | 710 | if(pitch == 1) step = direction_control; |
pyonta2017 | 0:6ddf1386e71d | 711 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 712 | break; |
pyonta2017 | 0:6ddf1386e71d | 713 | |
pyonta2017 | 0:6ddf1386e71d | 714 | case direction_control: |
pyonta2017 | 0:6ddf1386e71d | 715 | myled1 = 0; |
pyonta2017 | 0:6ddf1386e71d | 716 | myled3 = 0; |
pyonta2017 | 0:6ddf1386e71d | 717 | myled4 = 1; |
pyonta2017 | 0:6ddf1386e71d | 718 | //int drive_time; |
pyonta2017 | 0:6ddf1386e71d | 719 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 720 | timer.stop(); |
pyonta2017 | 0:6ddf1386e71d | 721 | timer.start(); |
pyonta2017 | 0:6ddf1386e71d | 722 | val = 0; |
pyonta2017 | 1:8a25883c423c | 723 | pin21.write(1.0); |
pyonta2017 | 1:8a25883c423c | 724 | if(pitch == 1){ |
pyonta2017 | 1:8a25883c423c | 725 | controlmotor1.write(0); |
pyonta2017 | 1:8a25883c423c | 726 | controlmotor2.write(1); |
pyonta2017 | 1:8a25883c423c | 727 | }else{ |
pyonta2017 | 1:8a25883c423c | 728 | controlmotor1.write(1); |
pyonta2017 | 1:8a25883c423c | 729 | controlmotor2.write(0); |
pyonta2017 | 1:8a25883c423c | 730 | } |
pyonta2017 | 1:8a25883c423c | 731 | wait(3); |
pyonta2017 | 0:6ddf1386e71d | 732 | controlmotor1.write(0); |
pyonta2017 | 0:6ddf1386e71d | 733 | controlmotor2.write(0); |
pyonta2017 | 0:6ddf1386e71d | 734 | pin21.write(0.0); |
pyonta2017 | 0:6ddf1386e71d | 735 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 736 | timer.stop(); |
pyonta2017 | 0:6ddf1386e71d | 737 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 738 | //step = jump; |
pyonta2017 | 1:8a25883c423c | 739 | if(jump_count == 2){ |
pyonta2017 | 0:6ddf1386e71d | 740 | step = jump; |
pyonta2017 | 0:6ddf1386e71d | 741 | } |
pyonta2017 | 0:6ddf1386e71d | 742 | else{ |
pyonta2017 | 0:6ddf1386e71d | 743 | step = attitude_determination; |
pyonta2017 | 0:6ddf1386e71d | 744 | } |
pyonta2017 | 0:6ddf1386e71d | 745 | jump_count = jump_count+1; |
pyonta2017 | 0:6ddf1386e71d | 746 | break; |
pyonta2017 | 0:6ddf1386e71d | 747 | |
pyonta2017 | 0:6ddf1386e71d | 748 | case jump: |
pyonta2017 | 0:6ddf1386e71d | 749 | //ログファイル |
pyonta2017 | 0:6ddf1386e71d | 750 | myled1 = 1; |
pyonta2017 | 0:6ddf1386e71d | 751 | myled2 = 1; |
pyonta2017 | 0:6ddf1386e71d | 752 | wait(2); |
pyonta2017 | 1:8a25883c423c | 753 | FILE *fp = fopen("/sd/mydir/height.txt", "a"); |
pyonta2017 | 0:6ddf1386e71d | 754 | wait(1); |
pyonta2017 | 0:6ddf1386e71d | 755 | timer.reset(); |
pyonta2017 | 0:6ddf1386e71d | 756 | timer.start(); |
pyonta2017 | 0:6ddf1386e71d | 757 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 758 | jumpmotor = 1; |
pyonta2017 | 0:6ddf1386e71d | 759 | Mu2.printf("@DT04JUMP\r\n"); |
pyonta2017 | 0:6ddf1386e71d | 760 | while(val < 4000){ |
pyonta2017 | 0:6ddf1386e71d | 761 | val = timer.read_ms(); |
pyonta2017 | 1:8a25883c423c | 762 | if(fp == NULL) { |
pyonta2017 | 1:8a25883c423c | 763 | jumpmotor = 1; |
pyonta2017 | 1:8a25883c423c | 764 | } |
pyonta2017 | 1:8a25883c423c | 765 | else{ |
pyonta2017 | 1:8a25883c423c | 766 | adxl[0] = 0x32; |
pyonta2017 | 1:8a25883c423c | 767 | i2c.write(addr,adxl,1); |
pyonta2017 | 1:8a25883c423c | 768 | i2c.read(0xA7,adxl,6); |
pyonta2017 | 1:8a25883c423c | 769 | axout = (((int16_t )adxl[1]) << 8) | adxl[0]; |
pyonta2017 | 1:8a25883c423c | 770 | ayout = (((int16_t )adxl[3]) << 8) | adxl[2]; |
pyonta2017 | 1:8a25883c423c | 771 | azout = (((int16_t )adxl[5]) << 8) | adxl[4]; |
pyonta2017 | 1:8a25883c423c | 772 | fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); |
pyonta2017 | 1:8a25883c423c | 773 | wait_ms(2); |
pyonta2017 | 1:8a25883c423c | 774 | } |
pyonta2017 | 0:6ddf1386e71d | 775 | } |
pyonta2017 | 0:6ddf1386e71d | 776 | jumpmotor = 0; |
pyonta2017 | 0:6ddf1386e71d | 777 | wait(1); |
pyonta2017 | 1:8a25883c423c | 778 | if(fp != NULL)fclose(fp); |
pyonta2017 | 0:6ddf1386e71d | 779 | myled3 = 1; |
pyonta2017 | 1:8a25883c423c | 780 | val = 0; |
pyonta2017 | 0:6ddf1386e71d | 781 | jump_count = 0; |
pyonta2017 | 0:6ddf1386e71d | 782 | wait(3); |
pyonta2017 | 0:6ddf1386e71d | 783 | step = get_goaldirection; |
pyonta2017 | 0:6ddf1386e71d | 784 | break; |
pyonta2017 | 0:6ddf1386e71d | 785 | } |
pyonta2017 | 0:6ddf1386e71d | 786 | } |
pyonta2017 | 0:6ddf1386e71d | 787 | } |
pyonta2017 | 0:6ddf1386e71d | 788 | } |