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