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