QQQ / Mbed 2 deprecated qqq_izu_main_test

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?

UserRevisionLine numberNew 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 }