20.09.29_main program test
Dependencies: mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt
main.cpp@0:f3a52599183f, 2020-08-27 (annotated)
- Committer:
- imadaemi
- Date:
- Thu Aug 27 10:10:38 2020 +0000
- Revision:
- 0:f3a52599183f
- Child:
- 1:d208f4e139a9
20.08.27_main program test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
imadaemi | 0:f3a52599183f | 1 | #include "mbed.h" |
imadaemi | 0:f3a52599183f | 2 | #include "math.h" |
imadaemi | 0:f3a52599183f | 3 | |
imadaemi | 0:f3a52599183f | 4 | #include "PQLPS22HB.h" |
imadaemi | 0:f3a52599183f | 5 | #include "mpu9250_i2c.h" |
imadaemi | 0:f3a52599183f | 6 | #include "GPS_interrupt.h" |
imadaemi | 0:f3a52599183f | 7 | |
imadaemi | 0:f3a52599183f | 8 | #define ACC_RANGE _16G |
imadaemi | 0:f3a52599183f | 9 | #define GYRO_RANGE _2000DPS |
imadaemi | 0:f3a52599183f | 10 | #define SEA_LEVEL_PRESS 1029.3//投下前に設定 |
imadaemi | 0:f3a52599183f | 11 | #define CURRENT_LOCATION_PRESS 1023.8//投下前に設定 |
imadaemi | 0:f3a52599183f | 12 | #define CURRENT_LOCATION_TEMP 16.6//投下前に設定 |
imadaemi | 0:f3a52599183f | 13 | #define LAT_GOAL 33.594909//投下前に計測 |
imadaemi | 0:f3a52599183f | 14 | #define LON_GOAL 130.217779//投下前に計測 |
imadaemi | 0:f3a52599183f | 15 | #define PI 3.14159265358979323846 |
imadaemi | 0:f3a52599183f | 16 | #define R 6378137 |
imadaemi | 0:f3a52599183f | 17 | |
imadaemi | 0:f3a52599183f | 18 | Serial pc(USBTX,USBRX, 115200); |
imadaemi | 0:f3a52599183f | 19 | Serial xbee(p28, p27, 115200); |
imadaemi | 0:f3a52599183f | 20 | Serial openlog(p13, p14, 115200); |
imadaemi | 0:f3a52599183f | 21 | Serial gps_serial(p13, p14, 115200); |
imadaemi | 0:f3a52599183f | 22 | Serial xbee_raspi(p28, p27, 115200); |
imadaemi | 0:f3a52599183f | 23 | I2C i2c(p9, p10); |
imadaemi | 0:f3a52599183f | 24 | DigitalIn flightpin(p20); |
imadaemi | 0:f3a52599183f | 25 | DigitalOut pin(p22); |
imadaemi | 0:f3a52599183f | 26 | |
imadaemi | 0:f3a52599183f | 27 | PwmOut motor_left26(p26);//左IN2 |
imadaemi | 0:f3a52599183f | 28 | PwmOut motor_left25(p25);//左IN1 |
imadaemi | 0:f3a52599183f | 29 | PwmOut motor_right24(p24);//右IN1 |
imadaemi | 0:f3a52599183f | 30 | PwmOut motor_right23(p23);//右IN2 |
imadaemi | 0:f3a52599183f | 31 | |
imadaemi | 0:f3a52599183f | 32 | LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW); |
imadaemi | 0:f3a52599183f | 33 | mpu9250 mpu9250(i2c,AD0_HIGH); |
imadaemi | 0:f3a52599183f | 34 | GPS_interrupt gps(&gps_serial); |
imadaemi | 0:f3a52599183f | 35 | |
imadaemi | 0:f3a52599183f | 36 | Timer timer; |
imadaemi | 0:f3a52599183f | 37 | Timeout nich_timeout; |
imadaemi | 0:f3a52599183f | 38 | Timeout nich_wait_timeout; |
imadaemi | 0:f3a52599183f | 39 | Timeout motor_timeout; |
imadaemi | 0:f3a52599183f | 40 | Ticker tick_mpu9250; |
imadaemi | 0:f3a52599183f | 41 | Ticker tick_lps22hb; |
imadaemi | 0:f3a52599183f | 42 | Ticker tick_gps; |
imadaemi | 0:f3a52599183f | 43 | Ticker tick_time; |
imadaemi | 0:f3a52599183f | 44 | Ticker tick_data; |
imadaemi | 0:f3a52599183f | 45 | |
imadaemi | 0:f3a52599183f | 46 | void Phase1(); |
imadaemi | 0:f3a52599183f | 47 | void Phase2(); |
imadaemi | 0:f3a52599183f | 48 | void Phase3(); |
imadaemi | 0:f3a52599183f | 49 | void Phase4(); |
imadaemi | 0:f3a52599183f | 50 | void on_data_received(); |
imadaemi | 0:f3a52599183f | 51 | void GetData(); |
imadaemi | 0:f3a52599183f | 52 | void SetSensor(); |
imadaemi | 0:f3a52599183f | 53 | void SetMpu9250(); |
imadaemi | 0:f3a52599183f | 54 | void GetMpu9250(); |
imadaemi | 0:f3a52599183f | 55 | void SetLps22hb(); |
imadaemi | 0:f3a52599183f | 56 | void GetLps22hb(); |
imadaemi | 0:f3a52599183f | 57 | void SetGPS(); |
imadaemi | 0:f3a52599183f | 58 | void GetGPS(); |
imadaemi | 0:f3a52599183f | 59 | void CalculateAltitude(); |
imadaemi | 0:f3a52599183f | 60 | void FlightPin(); |
imadaemi | 0:f3a52599183f | 61 | void CutNichrome(float nich_wait); |
imadaemi | 0:f3a52599183f | 62 | void NichromeOn(); |
imadaemi | 0:f3a52599183f | 63 | void NichromeOff(); |
imadaemi | 0:f3a52599183f | 64 | void CalcDirection(float lat, float lon, float lat_f, float lon_f); |
imadaemi | 0:f3a52599183f | 65 | void CalcDistance(float lat, float lon); |
imadaemi | 0:f3a52599183f | 66 | void MotorFront(float f_left,float f_right,float f_wait); |
imadaemi | 0:f3a52599183f | 67 | void MotorBack(float f_left,float f_right,float f_wait); |
imadaemi | 0:f3a52599183f | 68 | void MotorStop(); |
imadaemi | 0:f3a52599183f | 69 | |
imadaemi | 0:f3a52599183f | 70 | int cansat_phase = 1; |
imadaemi | 0:f3a52599183f | 71 | bool mpu9250_test; |
imadaemi | 0:f3a52599183f | 72 | bool mag_mpu9250_test; |
imadaemi | 0:f3a52599183f | 73 | float imu[6], mag[3]; |
imadaemi | 0:f3a52599183f | 74 | float press, temp; |
imadaemi | 0:f3a52599183f | 75 | float altitude_ground, altitude; |
imadaemi | 0:f3a52599183f | 76 | float lat, lon, height; |
imadaemi | 0:f3a52599183f | 77 | bool nich_status = false; |
imadaemi | 0:f3a52599183f | 78 | float cansat_direction; |
imadaemi | 0:f3a52599183f | 79 | float cansat_distance; |
imadaemi | 0:f3a52599183f | 80 | float lat_0, lon_0; |
imadaemi | 0:f3a52599183f | 81 | int count_loop = 1; |
imadaemi | 0:f3a52599183f | 82 | |
imadaemi | 0:f3a52599183f | 83 | int main(){ |
imadaemi | 0:f3a52599183f | 84 | Phase4(); |
imadaemi | 0:f3a52599183f | 85 | } |
imadaemi | 0:f3a52599183f | 86 | |
imadaemi | 0:f3a52599183f | 87 | void Phase1(){ |
imadaemi | 0:f3a52599183f | 88 | pc.printf("Phase_%d\r\n", cansat_phase); |
imadaemi | 0:f3a52599183f | 89 | xbee.printf("Phase_%d\r\n", cansat_phase); |
imadaemi | 0:f3a52599183f | 90 | SetSensor(); |
imadaemi | 0:f3a52599183f | 91 | tick_data.attach(&GetData, 1.0); |
imadaemi | 0:f3a52599183f | 92 | cansat_phase = 2; |
imadaemi | 0:f3a52599183f | 93 | pc.printf("SetSensor finished!\r\n"); |
imadaemi | 0:f3a52599183f | 94 | xbee.printf("SetSensor finished!\r\n"); |
imadaemi | 0:f3a52599183f | 95 | } |
imadaemi | 0:f3a52599183f | 96 | |
imadaemi | 0:f3a52599183f | 97 | void Phase2(){//フライトピンの検知 |
imadaemi | 0:f3a52599183f | 98 | pc.printf("Phese_%d\r\n", cansat_phase); |
imadaemi | 0:f3a52599183f | 99 | xbee.printf("Phese_%d\r\n", cansat_phase); |
imadaemi | 0:f3a52599183f | 100 | timer.start(); |
imadaemi | 0:f3a52599183f | 101 | while(cansat_phase == 2){ |
imadaemi | 0:f3a52599183f | 102 | if(flightpin == 1){ |
imadaemi | 0:f3a52599183f | 103 | break; |
imadaemi | 0:f3a52599183f | 104 | } |
imadaemi | 0:f3a52599183f | 105 | } |
imadaemi | 0:f3a52599183f | 106 | cansat_phase = 3; |
imadaemi | 0:f3a52599183f | 107 | pc.printf("FlightPin finished!\r\n"); |
imadaemi | 0:f3a52599183f | 108 | xbee.printf("FlightPin finished!\r\n"); |
imadaemi | 0:f3a52599183f | 109 | } |
imadaemi | 0:f3a52599183f | 110 | |
imadaemi | 0:f3a52599183f | 111 | void Phase3(){//ニクロム線による分離 |
imadaemi | 0:f3a52599183f | 112 | pc.printf("Phase_%d\r\n", cansat_phase); |
imadaemi | 0:f3a52599183f | 113 | xbee.printf("Phase_%d\r\n", cansat_phase); |
imadaemi | 0:f3a52599183f | 114 | while(cansat_phase == 3){ |
imadaemi | 0:f3a52599183f | 115 | CutNichrome(2.0f); |
imadaemi | 0:f3a52599183f | 116 | break; |
imadaemi | 0:f3a52599183f | 117 | } |
imadaemi | 0:f3a52599183f | 118 | //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる |
imadaemi | 0:f3a52599183f | 119 | //MotorStop(); |
imadaemi | 0:f3a52599183f | 120 | cansat_phase = 4; |
imadaemi | 0:f3a52599183f | 121 | pc.printf("CutNichrome finished!\r\n"); |
imadaemi | 0:f3a52599183f | 122 | xbee.printf("CutNichrome finished!\r\n"); |
imadaemi | 0:f3a52599183f | 123 | } |
imadaemi | 0:f3a52599183f | 124 | |
imadaemi | 0:f3a52599183f | 125 | void Phase4(){//走行中 |
imadaemi | 0:f3a52599183f | 126 | wait(3.0f); |
imadaemi | 0:f3a52599183f | 127 | while(1){ |
imadaemi | 0:f3a52599183f | 128 | GetGPS(); |
imadaemi | 0:f3a52599183f | 129 | if(lat == 0){ |
imadaemi | 0:f3a52599183f | 130 | pc.printf("GPS Data NG...\r\n"); |
imadaemi | 0:f3a52599183f | 131 | xbee.printf("GPS Data NG...\r\n"); |
imadaemi | 0:f3a52599183f | 132 | }else{ |
imadaemi | 0:f3a52599183f | 133 | break; |
imadaemi | 0:f3a52599183f | 134 | } |
imadaemi | 0:f3a52599183f | 135 | } |
imadaemi | 0:f3a52599183f | 136 | |
imadaemi | 0:f3a52599183f | 137 | GetGPS(); |
imadaemi | 0:f3a52599183f | 138 | lat_0 = lat; |
imadaemi | 0:f3a52599183f | 139 | lon_0 = lon; |
imadaemi | 0:f3a52599183f | 140 | |
imadaemi | 0:f3a52599183f | 141 | MotorFront(1.0, 1.0, 3.0); |
imadaemi | 0:f3a52599183f | 142 | while(1){ |
imadaemi | 0:f3a52599183f | 143 | GetGPS(); |
imadaemi | 0:f3a52599183f | 144 | |
imadaemi | 0:f3a52599183f | 145 | xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); |
imadaemi | 0:f3a52599183f | 146 | |
imadaemi | 0:f3a52599183f | 147 | CalcDirection(lat, lon, lat_0, lon_0); |
imadaemi | 0:f3a52599183f | 148 | float direc_n = cansat_direction; |
imadaemi | 0:f3a52599183f | 149 | |
imadaemi | 0:f3a52599183f | 150 | xbee.printf("%f\t\r\n", direc_n); |
imadaemi | 0:f3a52599183f | 151 | |
imadaemi | 0:f3a52599183f | 152 | if(-90 < direc_n && direc_n <= 0){//右へ |
imadaemi | 0:f3a52599183f | 153 | pc.printf("Go to the Right\r\n"); |
imadaemi | 0:f3a52599183f | 154 | xbee.printf("Go to the Right-90\r\n"); |
imadaemi | 0:f3a52599183f | 155 | MotorFront(1.0, 0, 0.25); |
imadaemi | 0:f3a52599183f | 156 | |
imadaemi | 0:f3a52599183f | 157 | }else if(-180 < direc_n && direc_n<= -90){//右へ |
imadaemi | 0:f3a52599183f | 158 | pc.printf("Go to the Right\r\n"); |
imadaemi | 0:f3a52599183f | 159 | xbee.printf("Go to the Right-180\r\n"); |
imadaemi | 0:f3a52599183f | 160 | MotorFront(1.0, 0, 0.5); |
imadaemi | 0:f3a52599183f | 161 | |
imadaemi | 0:f3a52599183f | 162 | }else if(-270 < direc_n && direc_n <= -180){//左へ |
imadaemi | 0:f3a52599183f | 163 | pc.printf("Go to the Left\r\n"); |
imadaemi | 0:f3a52599183f | 164 | xbee.printf("Go to the Left-270\r\n"); |
imadaemi | 0:f3a52599183f | 165 | MotorFront(0, 1.0, 0.5); |
imadaemi | 0:f3a52599183f | 166 | |
imadaemi | 0:f3a52599183f | 167 | }else if(-360 <= direc_n && direc_n <= -270){//左へ |
imadaemi | 0:f3a52599183f | 168 | pc.printf("Go to the Left\r\n"); |
imadaemi | 0:f3a52599183f | 169 | xbee.printf("Go to the Left-360\r\n"); |
imadaemi | 0:f3a52599183f | 170 | MotorFront(0, 1.0, 0.25); |
imadaemi | 0:f3a52599183f | 171 | |
imadaemi | 0:f3a52599183f | 172 | }else if(0 < direc_n && direc_n<= 90){//左へ |
imadaemi | 0:f3a52599183f | 173 | pc.printf("Go to the Left\r\n"); |
imadaemi | 0:f3a52599183f | 174 | xbee.printf("Go to the Left90\r\n"); |
imadaemi | 0:f3a52599183f | 175 | MotorFront(0, 1.0, 0.25); |
imadaemi | 0:f3a52599183f | 176 | |
imadaemi | 0:f3a52599183f | 177 | }else if(90 < direc_n && direc_n <= 180){//左へ |
imadaemi | 0:f3a52599183f | 178 | pc.printf("Go to the Left\r\n"); |
imadaemi | 0:f3a52599183f | 179 | xbee.printf("Go to the Left180\r\n"); |
imadaemi | 0:f3a52599183f | 180 | MotorFront(0, 1.0, 0.5); |
imadaemi | 0:f3a52599183f | 181 | |
imadaemi | 0:f3a52599183f | 182 | }else if(180 < direc_n && direc_n<= 270){//右へ |
imadaemi | 0:f3a52599183f | 183 | pc.printf("Go to the Right\r\n"); |
imadaemi | 0:f3a52599183f | 184 | xbee.printf("Go to the Right270\r\n"); |
imadaemi | 0:f3a52599183f | 185 | MotorFront(1.0, 0, 0.5); |
imadaemi | 0:f3a52599183f | 186 | |
imadaemi | 0:f3a52599183f | 187 | }else if(270 < direc_n && direc_n <=360){//右へ |
imadaemi | 0:f3a52599183f | 188 | pc.printf("Go to the Right\r\n"); |
imadaemi | 0:f3a52599183f | 189 | xbee.printf("Go to the Right360\r\n"); |
imadaemi | 0:f3a52599183f | 190 | MotorFront(1.0, 0, 0.25); |
imadaemi | 0:f3a52599183f | 191 | } |
imadaemi | 0:f3a52599183f | 192 | |
imadaemi | 0:f3a52599183f | 193 | lat_0 = lat; |
imadaemi | 0:f3a52599183f | 194 | lon_0 = lon; |
imadaemi | 0:f3a52599183f | 195 | MotorFront(1.0, 1.0, 3.0); |
imadaemi | 0:f3a52599183f | 196 | MotorStop(); |
imadaemi | 0:f3a52599183f | 197 | wait(5.0); |
imadaemi | 0:f3a52599183f | 198 | |
imadaemi | 0:f3a52599183f | 199 | count_loop = count_loop + 1; |
imadaemi | 0:f3a52599183f | 200 | xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); |
imadaemi | 0:f3a52599183f | 201 | } |
imadaemi | 0:f3a52599183f | 202 | |
imadaemi | 0:f3a52599183f | 203 | } |
imadaemi | 0:f3a52599183f | 204 | |
imadaemi | 0:f3a52599183f | 205 | /* |
imadaemi | 0:f3a52599183f | 206 | void Phase5(){ |
imadaemi | 0:f3a52599183f | 207 | while(cansat_phase = 5){ |
imadaemi | 0:f3a52599183f | 208 | void on_data_received(); |
imadaemi | 0:f3a52599183f | 209 | } |
imadaemi | 0:f3a52599183f | 210 | } |
imadaemi | 0:f3a52599183f | 211 | */ |
imadaemi | 0:f3a52599183f | 212 | void on_data_received(){ |
imadaemi | 0:f3a52599183f | 213 | char data = xbee_raspi.getc(); |
imadaemi | 0:f3a52599183f | 214 | xbee_raspi.printf("%c", data); |
imadaemi | 0:f3a52599183f | 215 | switch(data) { |
imadaemi | 0:f3a52599183f | 216 | case 0x81://ゴール判定 |
imadaemi | 0:f3a52599183f | 217 | xbee_raspi.printf("red rate > 0.1\r\n"); |
imadaemi | 0:f3a52599183f | 218 | break; |
imadaemi | 0:f3a52599183f | 219 | case 0x82://ゴール発見 |
imadaemi | 0:f3a52599183f | 220 | xbee_raspi.printf("red rate > 0.01\r\n"); |
imadaemi | 0:f3a52599183f | 221 | break; |
imadaemi | 0:f3a52599183f | 222 | case 0x83://ゴールが見えない |
imadaemi | 0:f3a52599183f | 223 | xbee_raspi.printf("cannot find red\r\n"); |
imadaemi | 0:f3a52599183f | 224 | break; |
imadaemi | 0:f3a52599183f | 225 | } |
imadaemi | 0:f3a52599183f | 226 | } |
imadaemi | 0:f3a52599183f | 227 | /*************************************************** |
imadaemi | 0:f3a52599183f | 228 | センサのセットアップ |
imadaemi | 0:f3a52599183f | 229 | ***************************************************/ |
imadaemi | 0:f3a52599183f | 230 | void SetSensor(){ |
imadaemi | 0:f3a52599183f | 231 | SetMpu9250(); |
imadaemi | 0:f3a52599183f | 232 | SetLps22hb(); |
imadaemi | 0:f3a52599183f | 233 | SetGPS(); |
imadaemi | 0:f3a52599183f | 234 | } |
imadaemi | 0:f3a52599183f | 235 | |
imadaemi | 0:f3a52599183f | 236 | void SetMpu9250(){//MPU9250のセットアップ |
imadaemi | 0:f3a52599183f | 237 | mpu9250_test = mpu9250.sensorTest(); |
imadaemi | 0:f3a52599183f | 238 | mag_mpu9250_test = mpu9250.mag_sensorTest(); |
imadaemi | 0:f3a52599183f | 239 | if(mpu9250_test == true){ |
imadaemi | 0:f3a52599183f | 240 | pc.printf("MPU9250 OK!\r\n"); |
imadaemi | 0:f3a52599183f | 241 | xbee.printf("MPU9250 OK!\r\n"); |
imadaemi | 0:f3a52599183f | 242 | }else{ |
imadaemi | 0:f3a52599183f | 243 | pc.printf("MPU9250 NG...\r\n"); |
imadaemi | 0:f3a52599183f | 244 | xbee.printf("MPU9250 NG...\r\n"); |
imadaemi | 0:f3a52599183f | 245 | } |
imadaemi | 0:f3a52599183f | 246 | if(mag_mpu9250_test == true){ |
imadaemi | 0:f3a52599183f | 247 | pc.printf("MPU9250 MAG OK!\r\n"); |
imadaemi | 0:f3a52599183f | 248 | xbee.printf("MPU9250 MAG OK!\r\n"); |
imadaemi | 0:f3a52599183f | 249 | }else{ |
imadaemi | 0:f3a52599183f | 250 | pc.printf("MPU9250 MAG NG...\r\n"); |
imadaemi | 0:f3a52599183f | 251 | xbee.printf("MPU9250 MAG NG...\r\n"); |
imadaemi | 0:f3a52599183f | 252 | } |
imadaemi | 0:f3a52599183f | 253 | mpu9250.setAcc(ACC_RANGE); |
imadaemi | 0:f3a52599183f | 254 | mpu9250.setGyro(GYRO_RANGE); |
imadaemi | 0:f3a52599183f | 255 | mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012); |
imadaemi | 0:f3a52599183f | 256 | } |
imadaemi | 0:f3a52599183f | 257 | |
imadaemi | 0:f3a52599183f | 258 | void SetLps22hb(){//LPS22HBのセットアップ |
imadaemi | 0:f3a52599183f | 259 | lps22hb.begin(); |
imadaemi | 0:f3a52599183f | 260 | if(lps22hb.test() == true){ |
imadaemi | 0:f3a52599183f | 261 | pc.printf("LPS22HB OK!\r\n"); |
imadaemi | 0:f3a52599183f | 262 | xbee.printf("LPS22HB OK!\r\n"); |
imadaemi | 0:f3a52599183f | 263 | }else{ |
imadaemi | 0:f3a52599183f | 264 | pc.printf("LPS22HB NG...\r\n"); |
imadaemi | 0:f3a52599183f | 265 | xbee.printf("LPS22HB NG...\r\n"); |
imadaemi | 0:f3a52599183f | 266 | } |
imadaemi | 0:f3a52599183f | 267 | } |
imadaemi | 0:f3a52599183f | 268 | |
imadaemi | 0:f3a52599183f | 269 | void SetGPS(){//GPSのセットアップ |
imadaemi | 0:f3a52599183f | 270 | // gps.changeGPSFreq(10); 入れるとエラーが出る |
imadaemi | 0:f3a52599183f | 271 | pc.printf("GPS OK!\r\n"); |
imadaemi | 0:f3a52599183f | 272 | xbee.printf("GPS OK!\r\n"); |
imadaemi | 0:f3a52599183f | 273 | } |
imadaemi | 0:f3a52599183f | 274 | |
imadaemi | 0:f3a52599183f | 275 | /*************************************************** |
imadaemi | 0:f3a52599183f | 276 | データの取得 |
imadaemi | 0:f3a52599183f | 277 | ***************************************************/ |
imadaemi | 0:f3a52599183f | 278 | void GetData(){ |
imadaemi | 0:f3a52599183f | 279 | GetMpu9250(); |
imadaemi | 0:f3a52599183f | 280 | GetLps22hb(); |
imadaemi | 0:f3a52599183f | 281 | GetGPS(); |
imadaemi | 0:f3a52599183f | 282 | pc.printf("%f\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%.7f\t%.7f\t%f\t%f\t%f\t\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude, lat, lon, height, cansat_direction, cansat_distance); |
imadaemi | 0:f3a52599183f | 283 | openlog.printf("%f\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%.7f\t%.7f\t%f\t%f\t%f\t\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude, lat, lon, height, cansat_direction, cansat_distance); |
imadaemi | 0:f3a52599183f | 284 | } |
imadaemi | 0:f3a52599183f | 285 | |
imadaemi | 0:f3a52599183f | 286 | void GetMpu9250(){ |
imadaemi | 0:f3a52599183f | 287 | mpu9250.getAll(imu, mag); |
imadaemi | 0:f3a52599183f | 288 | //pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); |
imadaemi | 0:f3a52599183f | 289 | //xbee.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); |
imadaemi | 0:f3a52599183f | 290 | //openlog.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); |
imadaemi | 0:f3a52599183f | 291 | } |
imadaemi | 0:f3a52599183f | 292 | |
imadaemi | 0:f3a52599183f | 293 | void GetLps22hb(){ |
imadaemi | 0:f3a52599183f | 294 | lps22hb.read_press(&press); |
imadaemi | 0:f3a52599183f | 295 | lps22hb.read_temp(&temp); |
imadaemi | 0:f3a52599183f | 296 | //pc.printf("%f\t%f\t",press, temp); |
imadaemi | 0:f3a52599183f | 297 | } |
imadaemi | 0:f3a52599183f | 298 | |
imadaemi | 0:f3a52599183f | 299 | void GetGPS(){ |
imadaemi | 0:f3a52599183f | 300 | lat = gps.Latitude(); |
imadaemi | 0:f3a52599183f | 301 | lon = gps.Longitude(); |
imadaemi | 0:f3a52599183f | 302 | height = gps.Height(); |
imadaemi | 0:f3a52599183f | 303 | //pc.printf("%f\t%f\t%f\t", lat, lon, height); |
imadaemi | 0:f3a52599183f | 304 | //xbee.printf("%f\t%f\t%f\t", lat, lon, height); |
imadaemi | 0:f3a52599183f | 305 | //openlog.printf("%f\t%f\t%f\t", lat, lon, height); |
imadaemi | 0:f3a52599183f | 306 | } |
imadaemi | 0:f3a52599183f | 307 | |
imadaemi | 0:f3a52599183f | 308 | /*************************************************** |
imadaemi | 0:f3a52599183f | 309 | 制御計算 |
imadaemi | 0:f3a52599183f | 310 | ***************************************************/ |
imadaemi | 0:f3a52599183f | 311 | void CalculateAltitude(){ |
imadaemi | 0:f3a52599183f | 312 | lps22hb.read_press(&press); |
imadaemi | 0:f3a52599183f | 313 | lps22hb.read_temp(&temp); |
imadaemi | 0:f3a52599183f | 314 | altitude_ground = (powf(SEA_LEVEL_PRESS / CURRENT_LOCATION_PRESS, 1/5.257) - 1) * (CURRENT_LOCATION_TEMP + 273.15) / 0.0065; |
imadaemi | 0:f3a52599183f | 315 | altitude = (powf(SEA_LEVEL_PRESS / press, 1/5.257) - 1) * (temp + 273.15) / 0.0065 - altitude_ground; |
imadaemi | 0:f3a52599183f | 316 | pc.printf("%f\t\r\n",altitude); |
imadaemi | 0:f3a52599183f | 317 | } |
imadaemi | 0:f3a52599183f | 318 | |
imadaemi | 0:f3a52599183f | 319 | void FlightPin(){ |
imadaemi | 0:f3a52599183f | 320 | //pc.printf("Hello FlightPin!\r\n"); |
imadaemi | 0:f3a52599183f | 321 | while(cansat_phase == 2){ |
imadaemi | 0:f3a52599183f | 322 | if(flightpin == 1){ |
imadaemi | 0:f3a52599183f | 323 | break; |
imadaemi | 0:f3a52599183f | 324 | } |
imadaemi | 0:f3a52599183f | 325 | } |
imadaemi | 0:f3a52599183f | 326 | //pc.printf("Goodbye FlightPin!\r\n"); |
imadaemi | 0:f3a52599183f | 327 | } |
imadaemi | 0:f3a52599183f | 328 | |
imadaemi | 0:f3a52599183f | 329 | void CutNichrome(float nich_wait){ |
imadaemi | 0:f3a52599183f | 330 | NichromeOn(); |
imadaemi | 0:f3a52599183f | 331 | wait(nich_wait); |
imadaemi | 0:f3a52599183f | 332 | NichromeOff(); |
imadaemi | 0:f3a52599183f | 333 | } |
imadaemi | 0:f3a52599183f | 334 | |
imadaemi | 0:f3a52599183f | 335 | void NichromeOn(){ |
imadaemi | 0:f3a52599183f | 336 | pin = 1; |
imadaemi | 0:f3a52599183f | 337 | nich_status = true; |
imadaemi | 0:f3a52599183f | 338 | } |
imadaemi | 0:f3a52599183f | 339 | |
imadaemi | 0:f3a52599183f | 340 | void NichromeOff(){ |
imadaemi | 0:f3a52599183f | 341 | pin = 0; |
imadaemi | 0:f3a52599183f | 342 | nich_status = false; |
imadaemi | 0:f3a52599183f | 343 | } |
imadaemi | 0:f3a52599183f | 344 | |
imadaemi | 0:f3a52599183f | 345 | void CalcDirection(float lat, float lon, float lat_f, float lon_f){ //ヒュベニの公式を使いたい |
imadaemi | 0:f3a52599183f | 346 | float dlat_g = (LAT_GOAL - lat) * (PI / 180); |
imadaemi | 0:f3a52599183f | 347 | float dlon_g = (LON_GOAL - lon) * (PI / 180); |
imadaemi | 0:f3a52599183f | 348 | float direc_g = atan2(dlat_g, dlon_g) * (180 / PI); |
imadaemi | 0:f3a52599183f | 349 | |
imadaemi | 0:f3a52599183f | 350 | float dlat = (lat - lat_f) * (PI / 180); |
imadaemi | 0:f3a52599183f | 351 | float dlon = (lon - lon_f) * (PI / 180); |
imadaemi | 0:f3a52599183f | 352 | float direc = atan2(dlat, dlon) * (180 / PI); |
imadaemi | 0:f3a52599183f | 353 | |
imadaemi | 0:f3a52599183f | 354 | cansat_direction = direc_g - direc; |
imadaemi | 0:f3a52599183f | 355 | } |
imadaemi | 0:f3a52599183f | 356 | |
imadaemi | 0:f3a52599183f | 357 | void CalcDistance(float lat, float lon){ |
imadaemi | 0:f3a52599183f | 358 | float dlat_distance = R * (LAT_GOAL - lat) * (PI / 180); |
imadaemi | 0:f3a52599183f | 359 | float dlon_distance = R * (LON_GOAL - lon) * (PI / 180) * cos(lat * (PI / 180)); |
imadaemi | 0:f3a52599183f | 360 | float distance_squared = dlat_distance * dlat_distance + dlon_distance * dlon_distance; |
imadaemi | 0:f3a52599183f | 361 | cansat_distance = sqrt(distance_squared); |
imadaemi | 0:f3a52599183f | 362 | } |
imadaemi | 0:f3a52599183f | 363 | |
imadaemi | 0:f3a52599183f | 364 | /*************************************************** |
imadaemi | 0:f3a52599183f | 365 | モーター制御 |
imadaemi | 0:f3a52599183f | 366 | ***************************************************/ |
imadaemi | 0:f3a52599183f | 367 | void MotorFront(float f_left,float f_right,float f_wait){ |
imadaemi | 0:f3a52599183f | 368 | motor_left26 = 0;//正転 |
imadaemi | 0:f3a52599183f | 369 | motor_left25 = f_left; |
imadaemi | 0:f3a52599183f | 370 | motor_right24 = f_right; |
imadaemi | 0:f3a52599183f | 371 | motor_right23 = 0; |
imadaemi | 0:f3a52599183f | 372 | wait(f_wait); |
imadaemi | 0:f3a52599183f | 373 | } |
imadaemi | 0:f3a52599183f | 374 | |
imadaemi | 0:f3a52599183f | 375 | void MotorBack(float b_left,float b_right,float b_wait){ |
imadaemi | 0:f3a52599183f | 376 | motor_left26 = b_left;//逆転 |
imadaemi | 0:f3a52599183f | 377 | motor_left25 = 0; |
imadaemi | 0:f3a52599183f | 378 | motor_right24 = 0; |
imadaemi | 0:f3a52599183f | 379 | motor_right23 = b_right; |
imadaemi | 0:f3a52599183f | 380 | wait(b_wait); |
imadaemi | 0:f3a52599183f | 381 | } |
imadaemi | 0:f3a52599183f | 382 | |
imadaemi | 0:f3a52599183f | 383 | void MotorStop(){ |
imadaemi | 0:f3a52599183f | 384 | motor_left26 = 0;//ストップ |
imadaemi | 0:f3a52599183f | 385 | motor_left25 = 0; |
imadaemi | 0:f3a52599183f | 386 | motor_right24 = 0; |
imadaemi | 0:f3a52599183f | 387 | motor_right23 = 0; |
imadaemi | 0:f3a52599183f | 388 | } |