種子島ロケットコンテスト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