Takuma Hayashi
種子島ロケットコンテスト2019で0mゴールした，Space Eggs -Stability-のプログラム
Dependencies: Nichrome_lib ADXL375_spi mbed Motor_lib LPS25H_spi GPS_interrupt
Diff: main.cpp
- Revision:
- 0:a38bf4917128
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 17 12:40:10 2020 +0000
@@ -0,0 +1,1150 @@
+#include "mbed.h"
+#include "math.h"
+#include "string.h"
+#include "GPS_interrupt.h"
+#include "LPS25H_spi.h"
+#include "Motor_lib.h"
+#include "Nichrome_lib.h"
+#include "ADXL375_spi.h"
+
+/*
+キャンパスコモン
+Lat : 33.594886
+Lon : 130.217847
+
+能代
+Lat : 40.14237977
+Lon : 139.9872893
+
+種コン：
+Lat : 30.37451499
+Lon : 130.95993809
+*/
+
+double lat_target = 30.37451499;
+double lon_target = 130.95993809;
+
+bool waitGPS = true;
+bool flag_RPi_setting = true;
+
+char file_name[32] = "/local/stable2.csv";
+
+bool cv_yel = true;
+
+/************************************************
+コンストラクタ
+************************************************/
+Serial pc(USBTX, USBRX, 115200);
+Serial GPS_serial(p9, p10, 38400);
+Serial XB(p28, p27, 115200);
+Serial RPi(p13, p14, 115200);
+SPI wspi(p5, p6, p7);
+LocalFileSystem local("local");
+FILE *fp; //ローカルファイル
+
+LPS25H_spi LPS25H(wspi, p11);
+ADXL375_spi ADXL375(wspi, p8);
+GPS_interrupt GPS(&GPS_serial);
+Nichrome_lib Nichrome1(p20);
+Nichrome_lib Nichrome2(p19);
+Motor_lib MotorR(p24, p23);
+Motor_lib MotorL(p21, p22);
+
+AnalogIn Vinpin(p15);
+InterruptIn GPSAve(p17);
+InterruptIn Button(p16);
+InterruptIn FlightPin(p18);
+
+Ticker tick_sensor;
+Ticker tick_GPS;
+Ticker sendTick_XB;
+Ticker sendTick_SD;
+Ticker sendTick_RPi;
+
+Timer timer;
+
+/************************************************
+関数のプロトタイプ宣言
+************************************************/
+void setup(); //セットアップ
+
+void phase1(); //準備～キャリア待機
+void phase2(); //降下中
+void phase3(); //分離・展開
+void phase4(); //走行（GPS）
+void phase5(); //走行（カメラ）
+void phase6(); //回収待機
+void flight(); //割り込み：フライトモード移行
+
+int setRTG(); //ゴール方向を計算
+float setLTG(); //ゴールまでの距離を計算
+void escape(); //スタック脱出
+void escape2();
+void activate(); //旋回
+
+void readGPS(); //GPSの座標読み取り
+void readSensor(); //センサーの値の取得
+void resetPT(); //割り込み：0m地点での気圧・温度のリセット
+
+void readRPi(); //RPiからのデータ読み取り
+void sendRPi(); //RPiにデータ送信
+void readCommand(); //RPiコマンド解析
+
+void ConnectCheck(); //割り込み：アクチュエータチェック
+void startGPSAve(); //割り込み：GPS平均計算開始
+void stopGPSAve(); //割り込み：GPS平均計算終了
+void GPSAverage(); //GPS平均計算
+void stopInterruptIn(); //割り込み：ピン割り込み停止
+void buttonPush(); //割り込み：ボタンを押したとき
+void buttonRelease();//割り込み：ボタンを離したとき
+
+void printData(); //PCでデータ表示
+void sendData(); //地上局へデータ送信
+void recData(); //ログ保存
+void startRec(); //記録開始
+void stopRec(); //記録終了
+
+/************************************************
+変数の宣言
+************************************************/
+double gps_lat, gps_lon, lat_0, lon_0, gps_alt, gps_knot, gps_deg;
+double gps_lat_raw[4], gps_lon_raw[4];
+int raw_count = 0;
+float utc[6];
+int gps_sat;
+bool flag_ave = false;
+double lat_sum, lon_sum, lat_ave, lon_ave, csum;
+
+float pres, temp, alt;
+float pres_0, temp_0;
+int sep_count = 0;
+
+float acc[3], gravity;
+
+float vin;
+
+int CanSat = 1; //CanSat mode
+int w_time; //wait timer
+int rtg; //rad to goal
+double ltg; //length to goal
+int reset_count;
+
+int f_time = 20; //分離タイマー
+
+bool esc = false;
+bool save = false;
+bool flight_yet = false;
+bool safety = false;
+bool nichrome1_yet = false;
+bool interrupt = false; //割り込み処理中
+bool button_push = false;
+bool phase4_once = false;
+
+char RPi_read[128];
+int RPi_pt = 0; //RPiコマンド文字列のポインタ
+int red_per, red_center_x, red_center_y, red_moment_x, red_moment_y, yel_per; //画像認識のデータ
+int laser; //距離センサのデータ
+
+/************************************************
+メイン関数
+************************************************/
+int main() {
+ setup();
+
+ RPi.attach(&readRPi, Serial::RxIrq);
+
+ tick_GPS.attach(&readGPS, 0.25f);
+ tick_sensor.attach(&readSensor, 0.05f);
+
+ phase1(); //準備～キャリア待機
+
+ phase2(); //降下中
+
+ phase3(); //分離・展開
+
+ phase4(); //走行（GPS）
+
+ phase5(); //走行（カメラ）
+
+ phase6(); //回収待機
+}
+
+/************************************************
+Phase1
+準備～キャリア待機中
+************************************************/
+void phase1(){
+ sendData();
+ recData();
+ sendRPi();
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+ sendTick_RPi.attach(&sendRPi, 2.0f);
+
+ FlightPin.mode(PullUp);
+ FlightPin.rise(&flight);
+
+ Button.mode(PullUp);
+ Button.fall(&buttonPush);
+ Button.rise(&buttonRelease);
+
+ GPSAve.mode(PullUp);
+ GPSAve.fall(&startGPSAve);
+ GPSAve.rise(&stopGPSAve);
+
+ w_time = 0;
+
+ while(CanSat == 1){
+ pc.printf("");
+ }
+}
+
+/************************************************
+Phase2
+降下中
+************************************************/
+void phase2(){
+ FlightPin.fall(&stopInterruptIn);
+ Button.fall(&stopInterruptIn);
+ Button.rise(&stopInterruptIn);
+ GPSAve.fall(&stopInterruptIn);
+ GPSAve.rise(&stopInterruptIn);
+
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendData();
+ recData();
+ sendTick_XB.attach(&sendData, 0.25f);
+ sendTick_SD.attach(&recData, 0.1f);
+
+ //Timer timer;
+ timer.reset();
+ timer.start();
+ //int count_down = 0;
+
+ sendRPi();
+
+ while(CanSat == 2){
+ w_time = f_time - timer.read();
+
+ //フライト後すぐにスタビ展開///////////////////////////////////////////////
+ if(!nichrome1_yet){
+ Nichrome1.fire(5.0f);
+ //pc.printf("Separate 1\r\n");
+ nichrome1_yet = true;
+ }
+ //ここまで//////////////////////////////////////////////////////////////
+
+ if(sep_count > 10 || w_time == 0){
+ if(!Nichrome1.status){
+ CanSat = 3;
+ timer.stop();
+ timer.reset();
+ w_time = 0;
+ }
+ }
+ }
+}
+
+/************************************************
+Phase3
+分離・展開
+************************************************/
+void phase3(){
+ while(CanSat == 3){
+ if(!Nichrome1.status){
+ Nichrome2.fire(5.0f);
+ //pc.printf("Separate 2\r\n");
+ timer.reset();
+ timer.start();
+ do{
+ w_time = 6 - timer.read();
+ }while(w_time > 0);
+ timer.stop();
+ timer.reset();
+ w_time = 0;
+ CanSat = 4;
+ }
+ }
+}
+
+/************************************************
+Phase4
+GPS走行中
+************************************************/
+void phase4(){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendData();
+ recData();
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+
+ if(!phase4_once){
+ phase4_once = true;
+ //パラが引っかからないようにスタート
+ for(int i = 0; i < 10; i++){
+ MotorL.turn_a(0.8f);
+ MotorR.turn_a(0.8f);
+ wait(0.1f);
+ MotorL.stop();
+ MotorR.stop();
+ wait(0.3f);
+ }
+ //ここまで
+
+ //30s待機
+ w_time = 30;
+ timer.reset();
+ timer.start();
+ while(w_time > 0){
+ w_time = 30 - timer.read();
+ }
+ }
+
+ //移動開始
+ MotorL.turn_a(0.8f);
+ MotorR.turn_a(1.0f);
+ timer.reset();
+ timer.start();
+ w_time = 3.0f;
+ while(w_time >= 0){
+ w_time = 3.0 - timer.read();
+ }
+
+ while(CanSat == 4){
+ if(reset_count >= 5){
+ reset_count = 0;
+ if((abs(gps_lat - lat_0) <= 0.000005 && abs(gps_lon - lon_0) <= 0.000005) || gps_knot < 0.3f){
+ escape();
+ }
+ activate();
+
+ lat_0 = gps_lat;
+ lon_0 = gps_lon;
+ }
+
+ if(yel_per > 20 && cv_yel){
+ escape2();
+ MotorL.turn_a(0.8f);
+ MotorR.turn_a(1.0f);
+ }
+
+ if(abs(gps_lat - lat_target) < 0.00004 && abs(gps_lon - lon_target) < 0.00004){
+ CanSat = 5;
+ break;
+ }
+
+ wait(0.1f);
+ }
+}
+
+/************************************************
+Phase5
+画像認識走行中
+************************************************/
+void phase5(){
+ sendTick_RPi.detach();
+ sendRPi();
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+ sendTick_RPi.attach(&sendRPi, 0.25f);
+
+ int w_time_camera = 180;
+ timer.reset();
+ timer.start();
+
+ while(CanSat == 5){
+
+ w_time = w_time_camera - timer.read();
+ if(w_time <= 0){
+ CanSat = 6;
+ }
+
+ ////////////////////////////////////////////////////////////////////////天気を見て変える
+ if(red_per >= 1){
+ MotorL.turn_a(0.6f);
+ MotorR.turn_a(0.8f);
+ if(red_per >= 40){
+ CanSat = 6;
+ }
+ else if(red_per < 3){
+ wait(0.5f);
+ }
+ else if(red_per < 8){
+ wait(1.0f);
+ }
+ else if(red_per < 13){
+ wait(1.5f);
+ }
+ else{
+ wait(2.0f);
+ }
+ }
+ else{
+ MotorL.turn_a(0.3f);
+ MotorR.turn_a(0.6f);
+ }
+
+ /*
+ if((red_per >= 5 && red_center_x != -1) || red_per >= 10){
+ if(red_per >= 40){ //天候次第
+ CanSat = 6;
+ }
+ */
+ /*
+ MotorL.turn_a(0.4f); //グラウンド状況次第
+ MotorR.turn_a(0.2f);
+ wait(0.5f);
+ */
+ /*
+ MotorL.turn_a(0.6f);
+ MotorR.turn_a(0.8f);
+ wait(0.5f);
+ }
+ else{
+ MotorL.turn_a(0.3f);
+ MotorR.turn_a(0.6f);
+ }
+ */
+ }
+ MotorL.stop();
+ MotorR.stop();
+}
+
+/************************************************
+Phase6
+終了・回収待機中
+************************************************/
+void phase6(){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendTick_RPi.detach();
+ sendData();
+ recData();
+ sendRPi();
+ sendTick_XB.attach(&sendData, 5.0f);
+ sendTick_SD.attach(&recData, 5.0f);
+ sendTick_RPi.attach(&sendRPi, 1.0f);
+
+ Button.fall(&buttonPush);
+
+ while(CanSat == 6){
+ }
+}
+
+/************************************************
+割り込み：Phase1 -> Phase2
+************************************************/
+void flight(){
+ if(!interrupt && !flight_yet && safety){
+ CanSat = 2;
+ flight_yet = true;
+ }
+}
+
+/************************************************
+ゴール方向を計算
+************************************************/
+int setRTG(){
+ double target_rad0 = atan2(lat_target - gps_lat, lon_target - gps_lon) - atan2(gps_lat - lat_0, gps_lon - lon_0);
+ double target_rad1 = target_rad0 * 180 / atan(1.0) / 4;
+ int target_rad = (int)target_rad1;
+ if(target_rad > 180){
+ return target_rad - 360;
+ }
+ else if(target_rad < -180){
+ return target_rad + 360;
+ }
+ else{
+ return target_rad;
+ }
+}
+
+/************************************************
+ゴールまでの距離を計算
+************************************************/
+float setLTG(){
+ float r_earth = 40 * pow(10.0f, 6);
+ float LTG_lat = r_earth / 360 * (lat_target - gps_lat);
+ float LTG_lon = r_earth * cos(lat_target / 180 * 3.141592654) / 360 * (lon_target - gps_lon);
+
+ return sqrt(pow(LTG_lat, 2) + pow(LTG_lon, 2));
+}
+
+/************************************************
+スタック脱出
+************************************************/
+void escape(){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendTick_RPi.detach();
+ sendData();
+ recData();
+ sendRPi();
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+ sendTick_RPi.attach(&sendRPi, 1.0f);
+
+ Timer timer;
+
+ esc = true;
+ MotorL.break_stop();
+ MotorR.break_stop();
+ wait(0.5f);
+
+ timer.start();
+ MotorL.turn_b();
+ MotorR.turn_b();
+ do{
+ w_time = 4 - timer.read();
+ }while(w_time > 2);
+
+ MotorL.stop();
+ MotorR.turn_b();
+ do{
+ w_time = 4 - timer.read();
+ }while(w_time > 1);
+
+ MotorL.turn_b();
+ MotorR.stop();
+ do{
+ w_time = 4 - timer.read();
+ }while(w_time > 0);
+
+ MotorL.stop();
+ MotorR.stop();
+
+ esc = false;
+}
+
+void escape2(){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendTick_RPi.detach();
+ sendData();
+ recData();
+ sendRPi();
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+ sendTick_RPi.attach(&sendRPi, 1.0f);
+
+ esc = true;
+
+ MotorL.break_stop();
+ MotorR.break_stop();
+ wait(0.5f);
+ MotorL.turn_b();
+ MotorR.turn_b();
+ wait(0.5f);
+ MotorL.turn_b();
+ MotorR.stop();
+ wait(90.0 * 5 / 1000);
+
+ esc = false;
+}
+
+/************************************************
+旋回
+************************************************/
+void activate(){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendTick_RPi.detach();
+ sendData();
+ recData();
+ sendRPi();
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+ sendTick_RPi.attach(&sendRPi, 1.0f);
+
+ if(rtg > 0){
+ MotorL.stop();
+ MotorR.turn_a(1.0f);
+ wait((float)rtg * 5 / 1000);
+ w_time = 0;
+ }
+ else{
+ MotorL.turn_a(0.8f);
+ MotorR.stop();
+ wait((float)rtg * -5 / 1000);
+ w_time = 0;
+ }
+
+ MotorL.turn_a(0.8f);
+ MotorR.turn_a(1.0f);
+}
+
+/************************************************
+GPS座標読み取り
+************************************************/
+void readGPS(){
+ double gps_lat_now = GPS.Latitude();
+ double gps_lon_now = GPS.Longitude();
+ if(gps_lat_now > 28.0f && gps_lat_now < 33.0f){
+ if(gps_lon_now > 128.0f && gps_lon_now < 133.0f){
+ gps_lat = gps_lat_now;
+ gps_lon = gps_lon_now;
+ raw_count ++;
+ if(raw_count == 4){
+ raw_count = 0;
+ }
+ }
+ }
+
+ gps_alt = GPS.Height();
+ GPS.getUTC(utc);
+ utc[3] += 9;
+ if(utc[3] >= 24){
+ utc[3] -= 24;
+ }
+ gps_knot = GPS.Knot();
+ gps_deg = GPS.Degree();
+ gps_sat = GPS.Number();
+
+ ltg = setLTG();
+
+ if(CanSat == 1 && flag_ave){ //待機モード時のみ
+ GPSAverage();
+ }
+
+ if(CanSat == 4 && !esc){ //走行モード時のみ
+ rtg = setRTG();
+ if(raw_count == 0){//raw_count == 4でリセットだから1秒ごと
+ reset_count ++;
+ if(ltg < 15){
+ w_time = 3 - reset_count;
+ }
+ else{
+ w_time = 5 - reset_count;
+ }
+ }
+ }
+}
+
+/************************************************
+センサー読み取り
+************************************************/
+void readSensor(){
+ float pres_now = LPS25H.getPres();
+ if(pres_now > 500 && pres_now < 1500){
+ pres = pres_now;
+ temp = LPS25H.getTemp();
+ alt = LPS25H.getAlt(pres_0, temp_0);
+ }
+
+ if(CanSat == 2){
+ if(alt < 5.0f && alt > -10.0f){
+ sep_count ++;
+ }
+ }
+
+ ADXL375.getOutput(acc);
+ gravity = sqrt(pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2));
+
+ vin = Vinpin.read() * 8.4;
+}
+
+/************************************************
+0m地点での気圧・温度のリセット
+************************************************/
+void resetPT(){
+ pres_0 = pres;
+ temp_0 = temp;
+
+ safety = true;
+
+ save = !save;
+ if(save){
+ startRec();
+ }
+ else{
+ stopRec();
+ }
+}
+
+/************************************************
+RPiからのデータ読み取り
+************************************************/
+void readRPi(){
+ RPi_read[RPi_pt] = RPi.getc();
+ RPi_pt ++;
+
+ if(RPi_read[RPi_pt - 1] == '_'){
+ if(RPi_pt > 3){
+ RPi_read[RPi_pt - 1] = '\0';
+ readCommand();
+ }
+ RPi_pt = 0;
+ }
+}
+
+/************************************************
+RPiにデータ送信
+************************************************/
+void sendRPi(){
+ RPi.printf("%d", CanSat);
+}
+
+/************************************************
+RPiコマンド解析
+************************************************/
+void readCommand(){
+ char command[128];
+ strcpy(command, RPi_read);
+ sscanf(command, "%d,%d,%d,%d,%d,%d,%d", &red_per,
+ &red_center_x, &red_center_y,
+ &red_moment_x, &red_moment_y,
+ &yel_per, &laser);
+}
+
+/************************************************
+割り込み：アクチュエータチェック
+************************************************/
+void ConnectCheck(){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+ sendTick_RPi.detach();
+
+ FlightPin.rise(&stopInterruptIn);
+ Button.fall(&stopInterruptIn);
+ Button.rise(&stopInterruptIn);
+ GPSAve.fall(&stopInterruptIn);
+ GPSAve.rise(&stopInterruptIn);
+
+ Timer timer;
+
+ pc.printf("\r\nTEST MODE\r\n");
+ XB.printf("\r\nTEST MODE\r\n");
+ wait(1.0f);
+
+ MotorL.turn_a();
+ pc.printf("Motor Left : FRONT\r\n");
+ XB.printf("Motor Left : FRONT\r\n");
+ wait(1.0f);
+ MotorL.turn_b();
+ pc.printf("Motor Left : BACK\r\n");
+ XB.printf("Motor Left : BACK\r\n");
+ wait(1.0f);
+ MotorL.stop();
+
+ MotorR.turn_a();
+ pc.printf("Motor Right : FRONT\r\n");
+ XB.printf("Motor Right : FRONT\r\n");
+ wait(1.0f);
+ MotorR.turn_b();
+ pc.printf("Motor Right : BACK\r\n");
+ XB.printf("Motor Right : BACK\r\n");
+ wait(1.0f);
+ MotorR.stop();
+
+ Nichrome1.fire(3.0f);
+ timer.start();
+ do{
+ w_time = 6 - timer.read();
+ pc.printf("Nichrome 1 : %d %d\r", w_time, Nichrome1.status);
+ XB.printf("Nichrome 1 : %d %d\r", w_time, Nichrome1.status);
+ if(w_time == 3){
+ Nichrome1.fire_off();
+ }
+ wait(0.1f);
+ }while(w_time > 0);
+ timer.stop();
+ timer.reset();
+ pc.printf("\n");
+ XB.printf("\n");
+
+ Nichrome2.fire(3.0f);
+ timer.start();
+ do{
+ w_time = 6 - timer.read();
+ pc.printf("Nichrome 2 : %d %d\r", w_time, Nichrome2.status);
+ XB.printf("Nichrome 2 : %d %d\r", w_time, Nichrome2.status);
+ if(w_time == 3){
+ Nichrome2.fire_off();
+ }
+ wait(0.1f);
+ }while(w_time > 0);
+ timer.stop();
+ timer.reset();
+ pc.printf("\n");
+ XB.printf("\n");
+
+ w_time = 0;
+
+ pc.printf("Test Finish!\r\n\n");
+ XB.printf("Test Finish!\r\n\n");
+
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+ sendTick_RPi.attach(&sendRPi, 1.0f);
+
+ FlightPin.rise(&flight);
+ Button.fall(&buttonPush);
+ Button.rise(&buttonRelease);
+ GPSAve.fall(&startGPSAve);
+ GPSAve.rise(&stopGPSAve);
+}
+
+/************************************************
+割り込み：GPS平均計算開始
+************************************************/
+void startGPSAve(){
+ if(!interrupt){
+ sendTick_XB.detach();
+ sendTick_SD.detach();
+
+ flag_ave = true;
+ lat_sum = 0;
+ lon_sum = 0;
+ csum = 0;
+
+ interrupt = true;
+ }
+}
+
+/************************************************
+割り込み：GPS平均計算終了
+************************************************/
+void stopGPSAve(){
+ if(interrupt){
+ flag_ave = false;
+
+ pc.printf("GPS Average\r\nCount : %.0lf\r\nLAT : %lf(N)\r\nLON : %lf(E)\r\n", csum, lat_ave, lon_ave);
+ XB.printf("GPS Average\r\nCount : %.0lf\r\nLAT : %lf(N)\r\nLON : %lf(E)\r\n", csum, lat_ave, lon_ave);
+
+ wait(5.0f);
+
+ sendTick_XB.attach(&sendData, 1.0f);
+ sendTick_SD.attach(&recData, 1.0f);
+
+ interrupt = false;
+ }
+}
+
+/************************************************
+GPS平均計算
+************************************************/
+void GPSAverage(){
+ lat_sum += gps_lat;
+ lon_sum += gps_lon;
+ csum ++;
+ lat_ave = lat_sum / csum;
+ lon_ave = lon_sum / csum;
+
+ pc.printf("GPS AVE(%.0lf) : %lf(N) , %lf(E)\r\n", csum, lat_ave, lon_ave);
+ XB.printf("GPS AVE(%.0lf) : %lf(N) , %lf(E)\r\n", csum, lat_ave, lon_ave);
+}
+
+/************************************************
+ピン割り込み停止
+************************************************/
+void stopInterruptIn(){
+ pc.printf("Stopped Interrupt\r\n");
+}
+
+/************************************************
+割り込み：ボタンを押したとき
+************************************************/
+void buttonPush(){
+ if(!interrupt && !button_push){
+ timer.start();
+ button_push = true;
+ interrupt = true;
+ }
+}
+
+/************************************************
+割り込み：ボタンを離したとき
+************************************************/
+void buttonRelease(){
+ if(interrupt && button_push){
+ int time_button;
+ button_push = false;
+ time_button = timer.read();
+ timer.stop();
+ timer.reset();
+ if(time_button >= 3){
+ ConnectCheck();
+ }
+ else{
+ resetPT();
+ }
+ interrupt = false;
+ }
+}
+
+/************************************************
+PCでデータ表示
+************************************************/
+void printData(){
+ for(int c1 = 0; c1 < 20; c1 ++){
+ pc.printf("*");
+ }
+ pc.printf("\r\n");
+ pc.printf("MODE -> ");
+ switch(CanSat){
+ case 0:
+ pc.printf("Setting\r\n");
+ break;
+ case 1:
+ pc.printf("Wait\r\n");
+ break;
+ case 2:
+ pc.printf("Descend\r\n");
+ break;
+ case 3:
+ pc.printf("Separate\r\n");
+ break;
+ case 4:
+ pc.printf("GPS\r\n");
+ break;
+ case 5:
+ pc.printf("Camera\r\n");
+ break;
+ case 6:
+ pc.printf("Finish\r\n");
+ }
+ pc.printf("GPS -> %3.7f(N), %3.7f(E), %d\r\n", gps_lat, gps_lon, gps_sat);
+ pc.printf("TIME -> %d:%d:%02.2f\r\n", (int)utc[3], (int)utc[4], utc[5]);
+ pc.printf("LPS25H -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
+ pc.printf("W_TIME -> %d[s]\r\n", w_time);
+ pc.printf("Nichrome -> %d, %d\r\n", Nichrome1.status, Nichrome2.status);
+ wait(0.05f);
+ pc.printf("RTG -> %d[degree]\r\n", rtg);
+ pc.printf("LTG -> %.2lf[m]\r\n", ltg);
+ pc.printf("Motor -> %.2f(L), %.2f(R)\r\n", MotorL.status, MotorR.status);
+ pc.printf("Stack -> %d\r\n", esc);
+ pc.printf("Save -> %d\r\n", save);
+ pc.printf("Battery -> %.2f\r\n", vin);
+ pc.printf("Red PER -> %d\r\n", red_per);
+ pc.printf("Red TRI -> %d, %d\r\n", red_center_x, red_center_y);
+ pc.printf("Red MOM -> %d, %d\r\n", red_moment_x, red_moment_y);
+ pc.printf("Yellow -> %d\r\n", yel_per);
+ pc.printf("Laser -> %d\r\n\n\n", laser);
+}
+
+/************************************************
+地上局へデータ送信
+************************************************/
+void sendData(){
+ if(CanSat == 2 || CanSat == 3){
+ XB.printf("%d[MODE],\t", CanSat);
+ XB.printf("%d[s],\t%.4f[hPa],\t%.2f[m],\t", w_time, pres, alt);
+ XB.printf("%d,\t%d %d\r\n", sep_count, Nichrome1.status, Nichrome2.status);
+ }
+ else{
+ for(int c1 = 0; c1 < 20; c1 ++){
+ XB.printf("*");
+ }
+ XB.printf("\r\n");
+ XB.printf("MODE -> ");
+ switch(CanSat){
+ case 0:
+ XB.printf("Setting\r\n");
+ break;
+ case 1:
+ XB.printf("Wait\r\n");
+ break;
+ case 2:
+ XB.printf("Descend\r\n");
+ break;
+ case 3:
+ XB.printf("Separate\r\n");
+ break;
+ case 4:
+ XB.printf("GPS\r\n");
+ break;
+ case 5:
+ XB.printf("Camera\r\n");
+ break;
+ case 6:
+ XB.printf("Finish\r\n");
+ }
+ XB.printf("GPS -> %3.7f(N), %3.7f(E), %d\r\n", gps_lat, gps_lon, gps_sat);
+ XB.printf("TIME -> %d:%d:%02.2f\r\n", (int)utc[3], (int)utc[4], utc[5]);
+ XB.printf("LPS25H -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
+ XB.printf("W_TIME -> %d[s]\r\n", w_time);
+ XB.printf("Nichrome -> %d, %d\r\n", Nichrome1.status, Nichrome2.status);
+ wait(0.05f);
+ XB.printf("RTG -> %d[degree]\r\n", rtg);
+ XB.printf("LTG -> %.2lf[m]\r\n", ltg);
+ XB.printf("Motor -> %.2f(L), %.2f(R)\r\n", MotorL.status, MotorR.status);
+ XB.printf("Stack -> %d\r\n", esc);
+ XB.printf("Save -> %d\r\n", save);
+ XB.printf("Battery -> %.2f\r\n", vin);
+ wait(0.05f);
+ XB.printf("Red PER -> %d\r\n", red_per);
+ XB.printf("Red TRI -> %d, %d\r\n", red_center_x, red_center_y);
+ XB.printf("Red MOM -> %d, %d\r\n", red_moment_x, red_moment_y);
+ XB.printf("Yellow -> %d\r\n", yel_per);
+ XB.printf("Laser -> %d\r\n\n\n", laser);
+ }
+}
+
+/************************************************
+ログ保存
+************************************************/
+void recData(){
+ if(save){
+ //時間情報
+ fprintf(fp, "%d/%d/%d,", (int)utc[0], (int)utc[1], (int)utc[2]); //日付
+ fprintf(fp, "%d:%02d:%02.2f,", (int)utc[3], (int)utc[4], utc[5]); //時間
+ switch(CanSat){ //モード
+ case 0:
+ fprintf(fp, "Setting,");
+ break;
+ case 1:
+ fprintf(fp, "Wait,");
+ break;
+ case 2:
+ fprintf(fp, "Descend,");
+ break;
+ case 3:
+ fprintf(fp, "Separate,");
+ break;
+ case 4:
+ fprintf(fp, "GPS,");
+ break;
+ case 5:
+ fprintf(fp, "Camera,");
+ break;
+ case 6:
+ fprintf(fp, "Finish,");
+ }
+ //センサ情報
+ fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
+ fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
+ fprintf(fp, "%d,", gps_sat); //GPS衛星数
+ fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt); //気圧センサ
+ fprintf(fp, "%.2f,%.2f,%.2f,%.4f,", acc[0], acc[1], acc[2], gravity);//加速度センサ
+ fprintf(fp, "%.2f,", vin); //電源電圧
+ //制御情報
+ fprintf(fp, "%d,", w_time); //待機時間
+ fprintf(fp, "%d,", rtg); //RadToGoal（ゴールの方向）
+ fprintf(fp, "%.2lf,", ltg); //LengthToGal（ゴールまでの距離）
+ //アクチュエータ情報
+ fprintf(fp, "%d,%d,", Nichrome1.status, Nichrome2.status); //ニクロム線
+ fprintf(fp, "%.2f,%.2f,", MotorL.status, MotorR.status); //モーター
+ fprintf(fp, "%d,", esc); //スタックしているか
+ //ラズパイ情報
+ fprintf(fp, "%d,", red_per); //赤色の割合
+ fprintf(fp, "%d,%d,", red_center_x, red_center_y); //赤色三角形重心の座標
+ fprintf(fp, "%d,%d,", red_moment_x, red_moment_y); //赤色重心の座標
+ fprintf(fp, "%d,", yel_per);
+ fprintf(fp, "%d", laser); //距離センサ
+ fprintf(fp, "\r\n");
+ }
+}
+
+/************************************************
+記録開始
+************************************************/
+void startRec(){
+ fp = fopen(file_name, "a");
+ pc.printf("START SAVING\r\n");
+ XB.printf("START SAVING\r\n");
+ wait(1.0f);
+}
+
+/************************************************
+記録終了
+************************************************/
+void stopRec(){
+ fprintf(fp, "\r\n\n");
+ fclose(fp);
+ pc.printf("STOP SAVING\r\n");
+ XB.printf("STOP SAVING\r\n");
+ wait(1.0f);
+}
+
+/************************************************
+セットアップ（最初に1回実行）
+************************************************/
+void setup(){
+ wait(0.5f);
+ char setup_string[512];
+ char sprint_buff[64];
+
+ pc.printf("\r\n\nSetting Start\r\n");
+ XB.printf("\r\n\nSetting Start\r\n");
+ strcat(setup_string, "\r\n\nSetting Start\r\n");
+
+ //LPS25Hの設定
+ LPS25H.begin(25);
+ LPS25H.setFIFO(16);
+ if(LPS25H.whoAmI() == 1){
+ pc.printf("LPS25H : OK\r\n");
+ XB.printf("LPS25H : OK\r\n");
+ strcat(setup_string, "LPS25H : OK\r\n");
+ pres_0 = LPS25H.getPres();
+ temp_0 = LPS25H.getTemp();
+ }
+ else{
+ pc.printf("LPS25H : NG...\r\n");
+ XB.printf("LPS25H : NG...\r\n");
+ strcat(setup_string, "LPS25H : NG...\r\n");
+ }
+
+ //ADXL375の設定
+ ADXL375.setDataRate(ADXL375_3200HZ);
+ if(ADXL375.whoAmI() == 1){
+ pc.printf("ADXL375 : OK\r\n");
+ XB.printf("ADXL375 : OK\r\n");
+ strcat(setup_string, "ADXL375 : OK\r\n");
+ }
+ else{
+ pc.printf("ADXL375 : NG...\r\n");
+ XB.printf("ADXL375 : NG...\r\n");
+ strcat(setup_string, "ADXL375 : NG...\r\n");
+ }
+
+ ADXL375.offset(-0.3f, -0.6f, 0.3f);
+
+ //GPS受信待機
+ if(waitGPS){
+ w_time = 0;
+ while(!GPS.gps_readable){
+ pc.printf("GPS Waiting... : %d\r", w_time);
+ XB.printf("GPS Waiting... : %d\r", w_time);
+ w_time ++;
+ wait(1.0f);
+ }
+ pc.printf("GPS Waiting... : %d -> OK!!\r\n", w_time);
+ XB.printf("GPS Waiting... : %d -> OK!!\r\n", w_time);
+ sprintf(sprint_buff, "GPS Waiting... : %d -> OK!!\r\n", w_time);
+ strcat(setup_string, sprint_buff);
+ }
+
+ //RPi起動待機
+ if(flag_RPi_setting){
+ bool flag_RPi_ok = false;
+ w_time = 0;
+ while(!flag_RPi_ok){
+ if(RPi.readable()){
+ char c = RPi.getc();
+ if(c == '_'){
+ flag_RPi_ok = true;
+ }
+ }
+ pc.printf("RPi setting : %d\r", w_time);
+ XB.printf("RPi setting : %d\r", w_time);
+ w_time ++;
+ wait(1.0f);
+ }
+ pc.printf("RPi setting : %d -> OK!!\r\n", w_time);
+ XB.printf("RPi setting : %d -> OK!!\r\n", w_time);
+ sprintf(sprint_buff, "RPi setting : %d -> OK!!\r\n", w_time);
+ strcat(setup_string, sprint_buff);
+ }
+
+ //まとめ
+ pc.printf("All setting finished!! -> Start!!\r\n");
+ XB.printf("All setting finished!! -> Start!!\r\n");
+ strcat(setup_string, "All setting finished!! -> Start!!\r\n");
+
+ fp = fopen(file_name, "a");
+ fprintf(fp, setup_string);
+ fclose(fp);
+ setup_string[0] = NULL;
+}
\ No newline at end of file