CansatB_2021 / Mbed 2 deprecated cansat_integrated_2

修正済みby皆川

Dependencies:   mbed Servo cansat_integrated_2 BMP180

Dependents:   cansat_integrated_2

direction.h@0:e7b7def631c2, 2021-10-21 (annotated)

Committer:
minanao
Date:
Thu Oct 21 01:58:35 2021 +0000
Revision:
0:e7b7def631c2
Child:
1:bb89b58cfa0e
integrated program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
minanao 0:e7b7def631c2 1 #include "mbed.h"
minanao 0:e7b7def631c2 2 #include "getGPS.h"
minanao 0:e7b7def631c2 3 #include "MPU9250.h"
minanao 0:e7b7def631c2 4
minanao 0:e7b7def631c2 5 Serial pc(SERIAL_TX, SERIAL_RX);
minanao 0:e7b7def631c2 6 GPS gps(D1, D0);
minanao 0:e7b7def631c2 7
minanao 0:e7b7def631c2 8 float calculation_Θ(float x_0,float y_0,float x_1,float y_1,float x,float y){
minanao 0:e7b7def631c2 9 //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。
minanao 0:e7b7def631c2 10 float Θ_0 = atan((y_0 - y_1)/(x_0 - x_1)); //目的地の角度
minanao 0:e7b7def631c2 11
minanao 0:e7b7def631c2 12 if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
minanao 0:e7b7def631c2 13 Θ_0 = Θ_0 - 180;
minanao 0:e7b7def631c2 14 }
minanao 0:e7b7def631c2 15 if(y_0 - y_1 < 0 && x_0 - x_1 < 0){
minanao 0:e7b7def631c2 16 Θ_0 = Θ_0 + 180;
minanao 0:e7b7def631c2 17 }
minanao 0:e7b7def631c2 18
minanao 0:e7b7def631c2 19 float Omag_x,Omag_y; //地磁気センサのxy平面が描く円の中点
minanao 0:e7b7def631c2 20 float Θ_1 = atan((y - Omag_y)/(x- Omag_x));　//CanSatの角度
minanao 0:e7b7def631c2 21
minanao 0:e7b7def631c2 22 if(y - Omag_y > 0 && x- Omag_x < 0){
minanao 0:e7b7def631c2 23 Θ_1 = Θ_1 - 180;
minanao 0:e7b7def631c2 24 }
minanao 0:e7b7def631c2 25 if(y - Omag_y < 0 && x - Omag_x < 0){
minanao 0:e7b7def631c2 26 Θ_1 = Θ_1 + 180;
minanao 0:e7b7def631c2 27 }
minanao 0:e7b7def631c2 28
minanao 0:e7b7def631c2 29 Θ = Θ_0 - Θ_1; //CanSatから見た目的地の角度
minanao 0:e7b7def631c2 30 }
minanao 0:e7b7def631c2 31
minanao 0:e7b7def631c2 32 class direction()
minanao 0:e7b7def631c2 33 {
minanao 0:e7b7def631c2 34 public:
minanao 0:e7b7def631c2 35
minanao 0:e7b7def631c2 36 Serial pc(SERIAL_TX, SERIAL_RX);
minanao 0:e7b7def631c2 37 GPS gps(D1, D0);
minanao 0:e7b7def631c2 38
minanao 0:e7b7def631c2 39 float x_0 ,y_0; //目的地
minanao 0:e7b7def631c2 40
minanao 0:e7b7def631c2 41 MPU9250 mpu9250(D4, D5); // SDA, SCL
minanao 0:e7b7def631c2 42
minanao 0:e7b7def631c2 43 Serial pc(USBTX, USBRX, 115200);
minanao 0:e7b7def631c2 44
minanao 0:e7b7def631c2 45 volatile bool interrupt_flag = false;
minanao 0:e7b7def631c2 46
minanao 0:e7b7def631c2 47 void detect_shock(void)
minanao 0:e7b7def631c2 48 {
minanao 0:e7b7def631c2 49 interrupt_flag = true;
minanao 0:e7b7def631c2 50 }
minanao 0:e7b7def631c2 51
minanao 0:e7b7def631c2 52 while(1) {
minanao 0:e7b7def631c2 53 if(gps.getgps()) //現在地取得
minanao 0:e7b7def631c2 54 pc.printf("(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
minanao 0:e7b7def631c2 55
minanao 0:e7b7def631c2 56 else
minanao 0:e7b7def631c2 57 pc.printf("No data\r\n");//データ取得に失敗した場合
minanao 0:e7b7def631c2 58
minanao 0:e7b7def631c2 59 wait(1);
minanao 0:e7b7def631c2 60 }
minanao 0:e7b7def631c2 61
minanao 0:e7b7def631c2 62
minanao 0:e7b7def631c2 63
minanao 0:e7b7def631c2 64 mpu9250.resetMPU9250();
minanao 0:e7b7def631c2 65 mpu9250.setWakeOnMotion();
minanao 0:e7b7def631c2 66
minanao 0:e7b7def631c2 67 while(1)
minanao 0:e7b7def631c2 68 {
minanao 0:e7b7def631c2 69 if (interrupt_flag)
minanao 0:e7b7def631c2 70 {
minanao 0:e7b7def631c2 71
minanao 0:e7b7def631c2 72 pc.printf("detect\r\n");
minanao 0:e7b7def631c2 73 wait_ms(1000);
minanao 0:e7b7def631c2 74 interrupt_flag = false;
minanao 0:e7b7def631c2 75 }
minanao 0:e7b7def631c2 76
minanao 0:e7b7def631c2 77 /* get mag data*/
minanao 0:e7b7def631c2 78 int16_t mag[3];
minanao 0:e7b7def631c2 79 mpu9250.readMagData(mag);
minanao 0:e7b7def631c2 80 float mag_f[3];
minanao 0:e7b7def631c2 81 for (int i = 0; i < 3; ++i)
minanao 0:e7b7def631c2 82 mag_f[i] = float(mag[i]) * mpu9250.getMres();
minanao 0:e7b7def631c2 83
minanao 0:e7b7def631c2 84
minanao 0:e7b7def631c2 85
minanao 0:e7b7def631c2 86
minanao 0:e7b7def631c2 87
minanao 0:e7b7def631c2 88
minanao 0:e7b7def631c2 89
minanao 0:e7b7def631c2 90 /* serial */
minanao 0:e7b7def631c2 91
minanao 0:e7b7def631c2 92 pc.printf("mx:%3.3f\t", mag_f[0]);
minanao 0:e7b7def631c2 93 pc.printf("my:%3.3f\t", mag_f[1]);
minanao 0:e7b7def631c2 94 pc.printf("mz:%3.3f\t\n", mag_f[2]);
minanao 0:e7b7def631c2 95
minanao 0:e7b7def631c2 96
minanao 0:e7b7def631c2 97 pc.printf("\n\n---------------------------\r\n");
minanao 0:e7b7def631c2 98
minanao 0:e7b7def631c2 99 wait_ms(100);
minanao 0:e7b7def631c2 100 }
minanao 0:e7b7def631c2 101 }
minanao 0:e7b7def631c2 102
minanao 0:e7b7def631c2 103