SSLM1
/
5-6_checkmode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: 2_MPU9250 mbed 2_GPS_GMS6-CR6 2_LPS33HW 2_EEPROM 2_P7100
Diff: main.cpp
- Revision:
- 0:9fb816db0c8b
- Child:
- 1:80140aac4bec
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jul 27 12:39:33 2020 +0000
@@ -0,0 +1,376 @@
+#include "mbed.h"
+#include "math.h"
+#include "EEPROM.h"
+#include "LPS33HW.h"
+#include "P7100.h"
+#include "MPU9250.h"
+#include "GMS6_CR6.h"
+
+
+Timer t; //時間計測
+Serial pc(USBTX, USBRX);
+LPS33HW lps(p9, p10);
+EEPROM rom(p9, p10);
+MPU9250 mpu(p9, p10);
+P7100 p7100(p5, p6, p7, p8);
+Serial twe(p28, p27); //EFM:p28, p27
+GMS6_CR6 gps(p13, p14); //EFM:p13, p14
+
+DigitalIn Flight_Pin(p30);
+DigitalOut ThrustValve(p21);
+DigitalOut DepressValve(p22);
+DigitalOut LED_1(p23);
+DigitalOut LED_2(p24);
+DigitalOut LED_3(p25);
+DigitalOut LED_4(p26);
+
+void get_save_low(); //自作関数プロトタイプ宣言
+float get_save_high(); //自作関数プロトタイプ宣言
+void recovery(); //自作関数プロトタイプ宣言
+void check();
+
+char s[64]; //データ取得配列
+float accel[3], gyro[3]; //MPU9250data
+float AX, AY, AZ, GX, GY, GZ; //MPU9250data
+float T; //時刻データ
+float P; //気圧データ取得
+float V; //電圧取得
+float TP = 0.25 * V - 0.125; //タンク圧力換算
+
+int pointer_high = 0; //EEPROM保存アドレス(high)
+int pointer_low = 0; //EEPROM保存アドレス(low)
+
+float lat_north; //GPS緯度
+float lon_east; //GPS経度
+float GPS[2]; //GPSデータ格納配列
+
+//main
+int main()
+{
+ pc.baud(460800); //ボーレート変更
+ twe.baud(230400); //tweボーレート変更
+
+ ThrustValve = 0; //DigitalOut初期化
+ DepressValve = 0;
+ LED_1 = 0;
+ LED_2 = 0;
+ LED_3 = 0;
+ LED_4 = 0;
+
+ //判定用変数
+ double groundP = 0; //地上の気圧データ
+ double heightP = 0; //高度TBDmの気圧
+ double judgeP[5]; //判定用気圧データ
+ double pastP = 0; //一つ前の気圧データ
+ double dP[5]; //気圧データの差異
+ int judge = 0; //判定結果
+ int i = 0; //差異配列の番号
+ memset(judgeP, 0, 5);
+ memset(dP, 1, 5);
+
+ int n_dvalve = 0; //脱圧バルブの開閉回数
+
+ t.start(); //timer Start
+
+ //sensor Setup
+ lps.start(1); //気圧センサスタート
+ lps.start(1); //気圧センサスタート
+ wait(0.1);
+ mpu.start(); //MPU9250 start&setup
+ mpu.accelsetup(3);
+ mpu.gyrosetup(3);
+ mpu.AKsetup(1);
+ wait(0.2);
+
+ pc.printf("Power ON!\r\n");
+ twe.printf("Power ON!\r\n");
+ wait(2);
+
+ //Check mode
+ pc.printf("Check mode Start!\r\n");
+ twe.printf("Check mode Start!\r\n");
+ //valve check
+ ThrustValve = 1;wait(1);ThrustValve = 0;
+ wait(1);
+ ThrustValve = 1;wait(1);ThrustValve = 0;
+ wait(2);
+ DepressValve = 1;wait(1);DepressValve = 0;
+ wait(1);
+ DepressValve = 1;wait(1);DepressValve = 0;
+ wait(1);
+
+ for(int c = 0; c < 60 ; c++) {
+ LED_1 = 1;
+ LED_2 = 1;
+ LED_3 = 1;
+ LED_4 = 1;
+ check();
+ LED_1 = 0;
+ LED_2 = 0;
+ LED_3 = 0;
+ LED_4 = 0;
+ wait(1);
+ }
+
+
+
+ //地上の気圧取得&計算
+ pc.printf("Calibration Start!\r\n");
+ twe.printf("Calibration Start!\r\n");
+ for(int a = 0; a < 1000; a++) {
+ groundP = groundP + lps.data_read();
+ wait_ms(50);
+ }
+ groundP = groundP/1000; //地上の気圧データ
+ heightP = 0.998862 * groundP; //高度10mの気圧を計算
+
+
+
+ //Wait mode
+ pc.printf("Wait mode Start!\r\n");
+ twe.printf("Wait mode Start!\r\n");
+ LED_1 = 1;
+ while(Flight_Pin == 0) {
+ get_save_low(); //data get&save
+ wait_ms(950);
+ }
+ pc.printf("Wait mode Finish!\r\n");
+ twe.printf("Wait mode Finish!\r\n");
+ LED_1 = 0;
+
+
+
+ //Flight mode
+ pc.printf("Flight mode Start!\r\n");
+ twe.printf("Flight mode Start!\r\n");
+ LED_2 = 1;
+ while(1) {
+ judgeP[i] = 0; //初期化
+
+ for(int n = 0; n < 2; n++) { //2回に一回送信
+ judgeP[i] = judgeP[i] + get_save_high(); //data get&save
+ }
+ twe.printf("\r\n"); //送信トリガー
+
+ //判定
+ judgeP[i] = judgeP[i]/2; //判定用気圧を計算
+ dP[i] = judgeP[i] - pastP; //差異を取得
+ pastP = judgeP[i]; //判定用気圧値を過去データに置き換える
+ i++;
+ if(i == 5) {
+ i = 0;
+ }
+
+ if(dP[0] > 0.03 && dP[1] > 0.03 && dP[2] > 0.03 && dP[3] > 0.03 && dP[4] > 0.03) { //空中判定
+ if(judgeP[0] > heightP && judgeP[1] > heightP && judgeP[2] > heightP && judgeP[3] > heightP && judgeP[4] > heightP) { //高度判定
+ judge = 1;
+ break;
+ }
+ } else if(dP[0] <= 0 && dP[1] <= 0 && dP[2] <= 0 && dP[3] <= 0 && dP[4] <= 0) {
+ judge = 1;
+ break;
+ }
+ }
+ pc.printf("Flight mode Finish!\r\n");
+ twe.printf("Flight mode Finish!\r\n");
+ LED_2 = 0;
+
+
+
+ //thruster mode
+ if(judge == 1) {
+ pc.printf("Thruster mode Start!\r\n");
+ twe.printf("Thruster mode Start!\r\n");
+ LED_3 = 1;
+ ThrustValve = 1; //スラスタ電磁弁開
+ for(int k = 0; k < 250; k++) {
+ for(int n = 0; n < 2; n++) { //2回に一回送信
+ get_save_high(); //data get&save
+ }
+ twe.printf("\r\n"); //送信トリガー
+ }
+ ThrustValve = 0; //スラスタ電磁弁閉
+ pc.printf("Thruster mode Finish!\r\n");
+ twe.printf("Thruster mode Finish!\r\n");
+ }
+
+
+
+ //depress mode
+ pc.printf("Depress mode Start!\r\n");
+ twe.printf("Depress mode Start!\r\n");
+ LED_4 = 1;
+ memset(judgeP, 0, 5); //判定初期化
+ memset(dP, 1, 5);
+ pastP = 0;
+
+ while(1) {
+ DepressValve = 1; //脱圧電磁弁開
+
+ judgeP[i] = 0; //初期化
+
+ for(int k = 0; k < 150; k++) {
+ for(int n = 0; n < 2; n++) { //2回に一回送信
+ judgeP[i] = judgeP[i] + get_save_high(); //data get&save
+ }
+ twe.printf("\r\n"); //送信トリガー
+
+ //判定
+ judgeP[i] = judgeP[i]/2; //判定用気圧を計算
+ dP[i] = judgeP[i] - pastP; //差異を取得
+ pastP = judgeP[i]; //判定用気圧値を過去データに置き換える
+
+ if(dP[0] <= 0 && dP[1] <= 0 && dP[2] <= 0 && dP[3] <= 0 && dP[4] <= 0 && n_dvalve > 2 || n_dvalve == 4) { //着地判定
+ judge = 3;
+ break;
+ }
+ i++;
+ if(i == 5) {
+ i = 0;
+ }
+ }
+
+ DepressValve = 0; //脱圧電磁弁閉
+ wait_ms(100);
+ n_dvalve++;
+
+ if(judge == 3) {
+ break; //地上判定を通過したらループを抜ける
+ }
+ }
+ pc.printf("Depress mode Finish!\r\n");
+ twe.printf("Depress mode Finish!\r\n");
+ LED_4 = 0;
+
+
+
+ //recovery mode
+ pc.printf("Recovery mode Start!\r\n");
+ twe.printf("Recovery mode Start!\r\n");
+ LED_1 = 1;
+ LED_2 = 1;
+ LED_4 = 1;
+ while(1) {
+ recovery();
+ }
+ pc.printf("Recovery mode Finish!\r\n");
+ twe.printf("Recovery mode Finish!\r\n");
+}
+
+
+//自作関数
+void get_save_low()
+{
+ T = t.read(); //時刻データ取得
+ P = lps.data_read(); //気圧データ取得
+ V = p7100.v_read(); //電圧取得
+ TP = 0.25 * V - 0.125; //タンク圧力換算
+
+ mpu.accel_read(3, accel);
+ mpu.gyro_read(3, gyro);
+ AX = accel[0];
+ AY = accel[1];
+ AZ = accel[2];
+ GX = gyro[0];
+ GY = gyro[1];
+ GZ = gyro[2];
+
+ sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
+
+ rom.write_low(pointer_low, s, 64); // write tha data
+ pointer_low = pointer_low + 64; //アドレスずらし
+
+ pc.printf("%s\r\n", s); //check用
+
+ memset(s, '\0', 64 ); //初期化
+}
+
+float get_save_high()
+{
+ T = t.read(); //時刻データ取得
+ P = lps.data_read(); //気圧データ取得
+ V = p7100.v_read(); //電圧取得
+ TP = 0.25 * V - 0.125; //タンク圧力換算
+
+ mpu.accel_read(3, accel);
+ mpu.gyro_read(3, gyro);
+ AX = accel[0];
+ AY = accel[1];
+ AZ = accel[2];
+ GX = gyro[0];
+ GY = gyro[1];
+ GZ = gyro[2];
+
+ sprintf(s, "%8.3f %8.3f %6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
+
+ rom.write_high(pointer_high, s, 64); // write tha data
+
+ twe.printf("%8.3f %8.3f %6.3f %5.2f,", T, P, TP, AZ); //無線機送信
+
+ pointer_high = pointer_high + 64; //アドレスずらし
+
+ pc.printf("%s\r\n", s); //check用
+
+ memset(s, '\0', 64 ); //初期化
+
+ return P;
+}
+
+void recovery()
+{
+ T = t.read(); //時刻データ取得
+ V = p7100.v_read(); //電圧取得
+ TP = 0.25 * V - 0.125; //タンク圧力換算
+
+ sprintf(s, "%8.3f %6.3f\r\n", T, TP); //floatからchar*へ変換
+
+ rom.write_low(pointer_low, s, 64); // write tha data
+ pointer_low = pointer_low + 64; //アドレスずらし
+
+ pc.printf("%s\r\n", s); //check用
+ twe.printf("%8.3f %6.3f, ", T, TP); //無線機送信
+ memset(s, '\0', 64 ); //初期化
+
+ gps.read_solid(GPS); //測位データ取得
+ lat_north = GPS[0];
+ lon_east = GPS[1];
+
+ if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信
+ twe.printf("%f,%f", lat_north, lon_east); //送信
+ pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
+ }
+ twe.printf("\r\n"); //無線機トリガー
+ memset(GPS,'\0', 2);
+}
+
+void check()
+{
+ T = t.read(); //時刻データ取得
+ P = lps.data_read(); //気圧データ取得
+ V = p7100.v_read(); //電圧取得
+ TP = 0.25 * V - 0.125; //タンク圧力換算
+
+ mpu.accel_read(3, accel);
+ mpu.gyro_read(3, gyro);
+ AX = accel[0];
+ AY = accel[1];
+ AZ = accel[2];
+ GX = gyro[0];
+ GY = gyro[1];
+ GZ = gyro[2];
+
+ sprintf(s, "%8.3f %8.3f %6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
+ twe.printf("%s\r\n", s); //送信
+ pc.printf("%s\r\n\r\n", s); //check用
+ memset(s, '\0', 64 ); //初期化
+
+ gps.read_solid(GPS); //測位データ取得
+ lat_north = GPS[0];
+ lon_east = GPS[1];
+
+ if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信
+ twe.printf("%f,%f\r\n", lat_north, lon_east); //送信
+ pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
+ }
+ memset(GPS,'\0', 2);
+}
\ No newline at end of file