統合プログラム
Dependencies: mbed Servo BMP180
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?
User | Revision | Line number | New 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 |