種子島ロケットコンテスト2019で0mゴールした,Space Eggs -Stability-のプログラム
Dependencies: Nichrome_lib ADXL375_spi mbed Motor_lib LPS25H_spi GPS_interrupt
main.cpp
- Committer:
- Sigma884
- Date:
- 2020-03-17
- Revision:
- 1:3d50114a513b
- Parent:
- 0:a38bf4917128
File content as of revision 1:3d50114a513b:
#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; }