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