![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
種子島ロケットコンテスト2019で0mゴールした,Space Eggs -Stability-のプログラム
Dependencies: Nichrome_lib ADXL375_spi mbed Motor_lib LPS25H_spi GPS_interrupt
main.cpp@0:a38bf4917128, 2020-03-17 (annotated)
- Committer:
- Sigma884
- Date:
- Tue Mar 17 12:40:10 2020 +0000
- Revision:
- 0:a38bf4917128
Used in Tanegashima Rocket Contest 2019
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sigma884 | 0:a38bf4917128 | 1 | #include "mbed.h" |
Sigma884 | 0:a38bf4917128 | 2 | #include "math.h" |
Sigma884 | 0:a38bf4917128 | 3 | #include "string.h" |
Sigma884 | 0:a38bf4917128 | 4 | #include "GPS_interrupt.h" |
Sigma884 | 0:a38bf4917128 | 5 | #include "LPS25H_spi.h" |
Sigma884 | 0:a38bf4917128 | 6 | #include "Motor_lib.h" |
Sigma884 | 0:a38bf4917128 | 7 | #include "Nichrome_lib.h" |
Sigma884 | 0:a38bf4917128 | 8 | #include "ADXL375_spi.h" |
Sigma884 | 0:a38bf4917128 | 9 | |
Sigma884 | 0:a38bf4917128 | 10 | /* |
Sigma884 | 0:a38bf4917128 | 11 | キャンパスコモン |
Sigma884 | 0:a38bf4917128 | 12 | Lat : 33.594886 |
Sigma884 | 0:a38bf4917128 | 13 | Lon : 130.217847 |
Sigma884 | 0:a38bf4917128 | 14 | |
Sigma884 | 0:a38bf4917128 | 15 | 能代 |
Sigma884 | 0:a38bf4917128 | 16 | Lat : 40.14237977 |
Sigma884 | 0:a38bf4917128 | 17 | Lon : 139.9872893 |
Sigma884 | 0:a38bf4917128 | 18 | |
Sigma884 | 0:a38bf4917128 | 19 | 種コン: |
Sigma884 | 0:a38bf4917128 | 20 | Lat : 30.37451499 |
Sigma884 | 0:a38bf4917128 | 21 | Lon : 130.95993809 |
Sigma884 | 0:a38bf4917128 | 22 | */ |
Sigma884 | 0:a38bf4917128 | 23 | |
Sigma884 | 0:a38bf4917128 | 24 | double lat_target = 30.37451499; |
Sigma884 | 0:a38bf4917128 | 25 | double lon_target = 130.95993809; |
Sigma884 | 0:a38bf4917128 | 26 | |
Sigma884 | 0:a38bf4917128 | 27 | bool waitGPS = true; |
Sigma884 | 0:a38bf4917128 | 28 | bool flag_RPi_setting = true; |
Sigma884 | 0:a38bf4917128 | 29 | |
Sigma884 | 0:a38bf4917128 | 30 | char file_name[32] = "/local/stable2.csv"; |
Sigma884 | 0:a38bf4917128 | 31 | |
Sigma884 | 0:a38bf4917128 | 32 | bool cv_yel = true; |
Sigma884 | 0:a38bf4917128 | 33 | |
Sigma884 | 0:a38bf4917128 | 34 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 35 | コンストラクタ |
Sigma884 | 0:a38bf4917128 | 36 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 37 | Serial pc(USBTX, USBRX, 115200); |
Sigma884 | 0:a38bf4917128 | 38 | Serial GPS_serial(p9, p10, 38400); |
Sigma884 | 0:a38bf4917128 | 39 | Serial XB(p28, p27, 115200); |
Sigma884 | 0:a38bf4917128 | 40 | Serial RPi(p13, p14, 115200); |
Sigma884 | 0:a38bf4917128 | 41 | SPI wspi(p5, p6, p7); |
Sigma884 | 0:a38bf4917128 | 42 | LocalFileSystem local("local"); |
Sigma884 | 0:a38bf4917128 | 43 | FILE *fp; //ローカルファイル |
Sigma884 | 0:a38bf4917128 | 44 | |
Sigma884 | 0:a38bf4917128 | 45 | LPS25H_spi LPS25H(wspi, p11); |
Sigma884 | 0:a38bf4917128 | 46 | ADXL375_spi ADXL375(wspi, p8); |
Sigma884 | 0:a38bf4917128 | 47 | GPS_interrupt GPS(&GPS_serial); |
Sigma884 | 0:a38bf4917128 | 48 | Nichrome_lib Nichrome1(p20); |
Sigma884 | 0:a38bf4917128 | 49 | Nichrome_lib Nichrome2(p19); |
Sigma884 | 0:a38bf4917128 | 50 | Motor_lib MotorR(p24, p23); |
Sigma884 | 0:a38bf4917128 | 51 | Motor_lib MotorL(p21, p22); |
Sigma884 | 0:a38bf4917128 | 52 | |
Sigma884 | 0:a38bf4917128 | 53 | AnalogIn Vinpin(p15); |
Sigma884 | 0:a38bf4917128 | 54 | InterruptIn GPSAve(p17); |
Sigma884 | 0:a38bf4917128 | 55 | InterruptIn Button(p16); |
Sigma884 | 0:a38bf4917128 | 56 | InterruptIn FlightPin(p18); |
Sigma884 | 0:a38bf4917128 | 57 | |
Sigma884 | 0:a38bf4917128 | 58 | Ticker tick_sensor; |
Sigma884 | 0:a38bf4917128 | 59 | Ticker tick_GPS; |
Sigma884 | 0:a38bf4917128 | 60 | Ticker sendTick_XB; |
Sigma884 | 0:a38bf4917128 | 61 | Ticker sendTick_SD; |
Sigma884 | 0:a38bf4917128 | 62 | Ticker sendTick_RPi; |
Sigma884 | 0:a38bf4917128 | 63 | |
Sigma884 | 0:a38bf4917128 | 64 | Timer timer; |
Sigma884 | 0:a38bf4917128 | 65 | |
Sigma884 | 0:a38bf4917128 | 66 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 67 | 関数のプロトタイプ宣言 |
Sigma884 | 0:a38bf4917128 | 68 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 69 | void setup(); //セットアップ |
Sigma884 | 0:a38bf4917128 | 70 | |
Sigma884 | 0:a38bf4917128 | 71 | void phase1(); //準備~キャリア待機 |
Sigma884 | 0:a38bf4917128 | 72 | void phase2(); //降下中 |
Sigma884 | 0:a38bf4917128 | 73 | void phase3(); //分離・展開 |
Sigma884 | 0:a38bf4917128 | 74 | void phase4(); //走行(GPS) |
Sigma884 | 0:a38bf4917128 | 75 | void phase5(); //走行(カメラ) |
Sigma884 | 0:a38bf4917128 | 76 | void phase6(); //回収待機 |
Sigma884 | 0:a38bf4917128 | 77 | void flight(); //割り込み:フライトモード移行 |
Sigma884 | 0:a38bf4917128 | 78 | |
Sigma884 | 0:a38bf4917128 | 79 | int setRTG(); //ゴール方向を計算 |
Sigma884 | 0:a38bf4917128 | 80 | float setLTG(); //ゴールまでの距離を計算 |
Sigma884 | 0:a38bf4917128 | 81 | void escape(); //スタック脱出 |
Sigma884 | 0:a38bf4917128 | 82 | void escape2(); |
Sigma884 | 0:a38bf4917128 | 83 | void activate(); //旋回 |
Sigma884 | 0:a38bf4917128 | 84 | |
Sigma884 | 0:a38bf4917128 | 85 | void readGPS(); //GPSの座標読み取り |
Sigma884 | 0:a38bf4917128 | 86 | void readSensor(); //センサーの値の取得 |
Sigma884 | 0:a38bf4917128 | 87 | void resetPT(); //割り込み:0m地点での気圧・温度のリセット |
Sigma884 | 0:a38bf4917128 | 88 | |
Sigma884 | 0:a38bf4917128 | 89 | void readRPi(); //RPiからのデータ読み取り |
Sigma884 | 0:a38bf4917128 | 90 | void sendRPi(); //RPiにデータ送信 |
Sigma884 | 0:a38bf4917128 | 91 | void readCommand(); //RPiコマンド解析 |
Sigma884 | 0:a38bf4917128 | 92 | |
Sigma884 | 0:a38bf4917128 | 93 | void ConnectCheck(); //割り込み:アクチュエータチェック |
Sigma884 | 0:a38bf4917128 | 94 | void startGPSAve(); //割り込み:GPS平均計算開始 |
Sigma884 | 0:a38bf4917128 | 95 | void stopGPSAve(); //割り込み:GPS平均計算終了 |
Sigma884 | 0:a38bf4917128 | 96 | void GPSAverage(); //GPS平均計算 |
Sigma884 | 0:a38bf4917128 | 97 | void stopInterruptIn(); //割り込み:ピン割り込み停止 |
Sigma884 | 0:a38bf4917128 | 98 | void buttonPush(); //割り込み:ボタンを押したとき |
Sigma884 | 0:a38bf4917128 | 99 | void buttonRelease();//割り込み:ボタンを離したとき |
Sigma884 | 0:a38bf4917128 | 100 | |
Sigma884 | 0:a38bf4917128 | 101 | void printData(); //PCでデータ表示 |
Sigma884 | 0:a38bf4917128 | 102 | void sendData(); //地上局へデータ送信 |
Sigma884 | 0:a38bf4917128 | 103 | void recData(); //ログ保存 |
Sigma884 | 0:a38bf4917128 | 104 | void startRec(); //記録開始 |
Sigma884 | 0:a38bf4917128 | 105 | void stopRec(); //記録終了 |
Sigma884 | 0:a38bf4917128 | 106 | |
Sigma884 | 0:a38bf4917128 | 107 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 108 | 変数の宣言 |
Sigma884 | 0:a38bf4917128 | 109 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 110 | double gps_lat, gps_lon, lat_0, lon_0, gps_alt, gps_knot, gps_deg; |
Sigma884 | 0:a38bf4917128 | 111 | double gps_lat_raw[4], gps_lon_raw[4]; |
Sigma884 | 0:a38bf4917128 | 112 | int raw_count = 0; |
Sigma884 | 0:a38bf4917128 | 113 | float utc[6]; |
Sigma884 | 0:a38bf4917128 | 114 | int gps_sat; |
Sigma884 | 0:a38bf4917128 | 115 | bool flag_ave = false; |
Sigma884 | 0:a38bf4917128 | 116 | double lat_sum, lon_sum, lat_ave, lon_ave, csum; |
Sigma884 | 0:a38bf4917128 | 117 | |
Sigma884 | 0:a38bf4917128 | 118 | float pres, temp, alt; |
Sigma884 | 0:a38bf4917128 | 119 | float pres_0, temp_0; |
Sigma884 | 0:a38bf4917128 | 120 | int sep_count = 0; |
Sigma884 | 0:a38bf4917128 | 121 | |
Sigma884 | 0:a38bf4917128 | 122 | float acc[3], gravity; |
Sigma884 | 0:a38bf4917128 | 123 | |
Sigma884 | 0:a38bf4917128 | 124 | float vin; |
Sigma884 | 0:a38bf4917128 | 125 | |
Sigma884 | 0:a38bf4917128 | 126 | int CanSat = 1; //CanSat mode |
Sigma884 | 0:a38bf4917128 | 127 | int w_time; //wait timer |
Sigma884 | 0:a38bf4917128 | 128 | int rtg; //rad to goal |
Sigma884 | 0:a38bf4917128 | 129 | double ltg; //length to goal |
Sigma884 | 0:a38bf4917128 | 130 | int reset_count; |
Sigma884 | 0:a38bf4917128 | 131 | |
Sigma884 | 0:a38bf4917128 | 132 | int f_time = 20; //分離タイマー |
Sigma884 | 0:a38bf4917128 | 133 | |
Sigma884 | 0:a38bf4917128 | 134 | bool esc = false; |
Sigma884 | 0:a38bf4917128 | 135 | bool save = false; |
Sigma884 | 0:a38bf4917128 | 136 | bool flight_yet = false; |
Sigma884 | 0:a38bf4917128 | 137 | bool safety = false; |
Sigma884 | 0:a38bf4917128 | 138 | bool nichrome1_yet = false; |
Sigma884 | 0:a38bf4917128 | 139 | bool interrupt = false; //割り込み処理中 |
Sigma884 | 0:a38bf4917128 | 140 | bool button_push = false; |
Sigma884 | 0:a38bf4917128 | 141 | bool phase4_once = false; |
Sigma884 | 0:a38bf4917128 | 142 | |
Sigma884 | 0:a38bf4917128 | 143 | char RPi_read[128]; |
Sigma884 | 0:a38bf4917128 | 144 | int RPi_pt = 0; //RPiコマンド文字列のポインタ |
Sigma884 | 0:a38bf4917128 | 145 | int red_per, red_center_x, red_center_y, red_moment_x, red_moment_y, yel_per; //画像認識のデータ |
Sigma884 | 0:a38bf4917128 | 146 | int laser; //距離センサのデータ |
Sigma884 | 0:a38bf4917128 | 147 | |
Sigma884 | 0:a38bf4917128 | 148 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 149 | メイン関数 |
Sigma884 | 0:a38bf4917128 | 150 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 151 | int main() { |
Sigma884 | 0:a38bf4917128 | 152 | setup(); |
Sigma884 | 0:a38bf4917128 | 153 | |
Sigma884 | 0:a38bf4917128 | 154 | RPi.attach(&readRPi, Serial::RxIrq); |
Sigma884 | 0:a38bf4917128 | 155 | |
Sigma884 | 0:a38bf4917128 | 156 | tick_GPS.attach(&readGPS, 0.25f); |
Sigma884 | 0:a38bf4917128 | 157 | tick_sensor.attach(&readSensor, 0.05f); |
Sigma884 | 0:a38bf4917128 | 158 | |
Sigma884 | 0:a38bf4917128 | 159 | phase1(); //準備~キャリア待機 |
Sigma884 | 0:a38bf4917128 | 160 | |
Sigma884 | 0:a38bf4917128 | 161 | phase2(); //降下中 |
Sigma884 | 0:a38bf4917128 | 162 | |
Sigma884 | 0:a38bf4917128 | 163 | phase3(); //分離・展開 |
Sigma884 | 0:a38bf4917128 | 164 | |
Sigma884 | 0:a38bf4917128 | 165 | phase4(); //走行(GPS) |
Sigma884 | 0:a38bf4917128 | 166 | |
Sigma884 | 0:a38bf4917128 | 167 | phase5(); //走行(カメラ) |
Sigma884 | 0:a38bf4917128 | 168 | |
Sigma884 | 0:a38bf4917128 | 169 | phase6(); //回収待機 |
Sigma884 | 0:a38bf4917128 | 170 | } |
Sigma884 | 0:a38bf4917128 | 171 | |
Sigma884 | 0:a38bf4917128 | 172 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 173 | Phase1 |
Sigma884 | 0:a38bf4917128 | 174 | 準備~キャリア待機中 |
Sigma884 | 0:a38bf4917128 | 175 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 176 | void phase1(){ |
Sigma884 | 0:a38bf4917128 | 177 | sendData(); |
Sigma884 | 0:a38bf4917128 | 178 | recData(); |
Sigma884 | 0:a38bf4917128 | 179 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 180 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 181 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 182 | sendTick_RPi.attach(&sendRPi, 2.0f); |
Sigma884 | 0:a38bf4917128 | 183 | |
Sigma884 | 0:a38bf4917128 | 184 | FlightPin.mode(PullUp); |
Sigma884 | 0:a38bf4917128 | 185 | FlightPin.rise(&flight); |
Sigma884 | 0:a38bf4917128 | 186 | |
Sigma884 | 0:a38bf4917128 | 187 | Button.mode(PullUp); |
Sigma884 | 0:a38bf4917128 | 188 | Button.fall(&buttonPush); |
Sigma884 | 0:a38bf4917128 | 189 | Button.rise(&buttonRelease); |
Sigma884 | 0:a38bf4917128 | 190 | |
Sigma884 | 0:a38bf4917128 | 191 | GPSAve.mode(PullUp); |
Sigma884 | 0:a38bf4917128 | 192 | GPSAve.fall(&startGPSAve); |
Sigma884 | 0:a38bf4917128 | 193 | GPSAve.rise(&stopGPSAve); |
Sigma884 | 0:a38bf4917128 | 194 | |
Sigma884 | 0:a38bf4917128 | 195 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 196 | |
Sigma884 | 0:a38bf4917128 | 197 | while(CanSat == 1){ |
Sigma884 | 0:a38bf4917128 | 198 | pc.printf(""); |
Sigma884 | 0:a38bf4917128 | 199 | } |
Sigma884 | 0:a38bf4917128 | 200 | } |
Sigma884 | 0:a38bf4917128 | 201 | |
Sigma884 | 0:a38bf4917128 | 202 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 203 | Phase2 |
Sigma884 | 0:a38bf4917128 | 204 | 降下中 |
Sigma884 | 0:a38bf4917128 | 205 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 206 | void phase2(){ |
Sigma884 | 0:a38bf4917128 | 207 | FlightPin.fall(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 208 | Button.fall(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 209 | Button.rise(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 210 | GPSAve.fall(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 211 | GPSAve.rise(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 212 | |
Sigma884 | 0:a38bf4917128 | 213 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 214 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 215 | sendData(); |
Sigma884 | 0:a38bf4917128 | 216 | recData(); |
Sigma884 | 0:a38bf4917128 | 217 | sendTick_XB.attach(&sendData, 0.25f); |
Sigma884 | 0:a38bf4917128 | 218 | sendTick_SD.attach(&recData, 0.1f); |
Sigma884 | 0:a38bf4917128 | 219 | |
Sigma884 | 0:a38bf4917128 | 220 | //Timer timer; |
Sigma884 | 0:a38bf4917128 | 221 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 222 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 223 | //int count_down = 0; |
Sigma884 | 0:a38bf4917128 | 224 | |
Sigma884 | 0:a38bf4917128 | 225 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 226 | |
Sigma884 | 0:a38bf4917128 | 227 | while(CanSat == 2){ |
Sigma884 | 0:a38bf4917128 | 228 | w_time = f_time - timer.read(); |
Sigma884 | 0:a38bf4917128 | 229 | |
Sigma884 | 0:a38bf4917128 | 230 | //フライト後すぐにスタビ展開/////////////////////////////////////////////// |
Sigma884 | 0:a38bf4917128 | 231 | if(!nichrome1_yet){ |
Sigma884 | 0:a38bf4917128 | 232 | Nichrome1.fire(5.0f); |
Sigma884 | 0:a38bf4917128 | 233 | //pc.printf("Separate 1\r\n"); |
Sigma884 | 0:a38bf4917128 | 234 | nichrome1_yet = true; |
Sigma884 | 0:a38bf4917128 | 235 | } |
Sigma884 | 0:a38bf4917128 | 236 | //ここまで////////////////////////////////////////////////////////////// |
Sigma884 | 0:a38bf4917128 | 237 | |
Sigma884 | 0:a38bf4917128 | 238 | if(sep_count > 10 || w_time == 0){ |
Sigma884 | 0:a38bf4917128 | 239 | if(!Nichrome1.status){ |
Sigma884 | 0:a38bf4917128 | 240 | CanSat = 3; |
Sigma884 | 0:a38bf4917128 | 241 | timer.stop(); |
Sigma884 | 0:a38bf4917128 | 242 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 243 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 244 | } |
Sigma884 | 0:a38bf4917128 | 245 | } |
Sigma884 | 0:a38bf4917128 | 246 | } |
Sigma884 | 0:a38bf4917128 | 247 | } |
Sigma884 | 0:a38bf4917128 | 248 | |
Sigma884 | 0:a38bf4917128 | 249 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 250 | Phase3 |
Sigma884 | 0:a38bf4917128 | 251 | 分離・展開 |
Sigma884 | 0:a38bf4917128 | 252 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 253 | void phase3(){ |
Sigma884 | 0:a38bf4917128 | 254 | while(CanSat == 3){ |
Sigma884 | 0:a38bf4917128 | 255 | if(!Nichrome1.status){ |
Sigma884 | 0:a38bf4917128 | 256 | Nichrome2.fire(5.0f); |
Sigma884 | 0:a38bf4917128 | 257 | //pc.printf("Separate 2\r\n"); |
Sigma884 | 0:a38bf4917128 | 258 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 259 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 260 | do{ |
Sigma884 | 0:a38bf4917128 | 261 | w_time = 6 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 262 | }while(w_time > 0); |
Sigma884 | 0:a38bf4917128 | 263 | timer.stop(); |
Sigma884 | 0:a38bf4917128 | 264 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 265 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 266 | CanSat = 4; |
Sigma884 | 0:a38bf4917128 | 267 | } |
Sigma884 | 0:a38bf4917128 | 268 | } |
Sigma884 | 0:a38bf4917128 | 269 | } |
Sigma884 | 0:a38bf4917128 | 270 | |
Sigma884 | 0:a38bf4917128 | 271 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 272 | Phase4 |
Sigma884 | 0:a38bf4917128 | 273 | GPS走行中 |
Sigma884 | 0:a38bf4917128 | 274 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 275 | void phase4(){ |
Sigma884 | 0:a38bf4917128 | 276 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 277 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 278 | sendData(); |
Sigma884 | 0:a38bf4917128 | 279 | recData(); |
Sigma884 | 0:a38bf4917128 | 280 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 281 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 282 | |
Sigma884 | 0:a38bf4917128 | 283 | if(!phase4_once){ |
Sigma884 | 0:a38bf4917128 | 284 | phase4_once = true; |
Sigma884 | 0:a38bf4917128 | 285 | //パラが引っかからないようにスタート |
Sigma884 | 0:a38bf4917128 | 286 | for(int i = 0; i < 10; i++){ |
Sigma884 | 0:a38bf4917128 | 287 | MotorL.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 288 | MotorR.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 289 | wait(0.1f); |
Sigma884 | 0:a38bf4917128 | 290 | MotorL.stop(); |
Sigma884 | 0:a38bf4917128 | 291 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 292 | wait(0.3f); |
Sigma884 | 0:a38bf4917128 | 293 | } |
Sigma884 | 0:a38bf4917128 | 294 | //ここまで |
Sigma884 | 0:a38bf4917128 | 295 | |
Sigma884 | 0:a38bf4917128 | 296 | //30s待機 |
Sigma884 | 0:a38bf4917128 | 297 | w_time = 30; |
Sigma884 | 0:a38bf4917128 | 298 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 299 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 300 | while(w_time > 0){ |
Sigma884 | 0:a38bf4917128 | 301 | w_time = 30 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 302 | } |
Sigma884 | 0:a38bf4917128 | 303 | } |
Sigma884 | 0:a38bf4917128 | 304 | |
Sigma884 | 0:a38bf4917128 | 305 | //移動開始 |
Sigma884 | 0:a38bf4917128 | 306 | MotorL.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 307 | MotorR.turn_a(1.0f); |
Sigma884 | 0:a38bf4917128 | 308 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 309 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 310 | w_time = 3.0f; |
Sigma884 | 0:a38bf4917128 | 311 | while(w_time >= 0){ |
Sigma884 | 0:a38bf4917128 | 312 | w_time = 3.0 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 313 | } |
Sigma884 | 0:a38bf4917128 | 314 | |
Sigma884 | 0:a38bf4917128 | 315 | while(CanSat == 4){ |
Sigma884 | 0:a38bf4917128 | 316 | if(reset_count >= 5){ |
Sigma884 | 0:a38bf4917128 | 317 | reset_count = 0; |
Sigma884 | 0:a38bf4917128 | 318 | if((abs(gps_lat - lat_0) <= 0.000005 && abs(gps_lon - lon_0) <= 0.000005) || gps_knot < 0.3f){ |
Sigma884 | 0:a38bf4917128 | 319 | escape(); |
Sigma884 | 0:a38bf4917128 | 320 | } |
Sigma884 | 0:a38bf4917128 | 321 | activate(); |
Sigma884 | 0:a38bf4917128 | 322 | |
Sigma884 | 0:a38bf4917128 | 323 | lat_0 = gps_lat; |
Sigma884 | 0:a38bf4917128 | 324 | lon_0 = gps_lon; |
Sigma884 | 0:a38bf4917128 | 325 | } |
Sigma884 | 0:a38bf4917128 | 326 | |
Sigma884 | 0:a38bf4917128 | 327 | if(yel_per > 20 && cv_yel){ |
Sigma884 | 0:a38bf4917128 | 328 | escape2(); |
Sigma884 | 0:a38bf4917128 | 329 | MotorL.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 330 | MotorR.turn_a(1.0f); |
Sigma884 | 0:a38bf4917128 | 331 | } |
Sigma884 | 0:a38bf4917128 | 332 | |
Sigma884 | 0:a38bf4917128 | 333 | if(abs(gps_lat - lat_target) < 0.00004 && abs(gps_lon - lon_target) < 0.00004){ |
Sigma884 | 0:a38bf4917128 | 334 | CanSat = 5; |
Sigma884 | 0:a38bf4917128 | 335 | break; |
Sigma884 | 0:a38bf4917128 | 336 | } |
Sigma884 | 0:a38bf4917128 | 337 | |
Sigma884 | 0:a38bf4917128 | 338 | wait(0.1f); |
Sigma884 | 0:a38bf4917128 | 339 | } |
Sigma884 | 0:a38bf4917128 | 340 | } |
Sigma884 | 0:a38bf4917128 | 341 | |
Sigma884 | 0:a38bf4917128 | 342 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 343 | Phase5 |
Sigma884 | 0:a38bf4917128 | 344 | 画像認識走行中 |
Sigma884 | 0:a38bf4917128 | 345 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 346 | void phase5(){ |
Sigma884 | 0:a38bf4917128 | 347 | sendTick_RPi.detach(); |
Sigma884 | 0:a38bf4917128 | 348 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 349 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 350 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 351 | sendTick_RPi.attach(&sendRPi, 0.25f); |
Sigma884 | 0:a38bf4917128 | 352 | |
Sigma884 | 0:a38bf4917128 | 353 | int w_time_camera = 180; |
Sigma884 | 0:a38bf4917128 | 354 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 355 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 356 | |
Sigma884 | 0:a38bf4917128 | 357 | while(CanSat == 5){ |
Sigma884 | 0:a38bf4917128 | 358 | |
Sigma884 | 0:a38bf4917128 | 359 | w_time = w_time_camera - timer.read(); |
Sigma884 | 0:a38bf4917128 | 360 | if(w_time <= 0){ |
Sigma884 | 0:a38bf4917128 | 361 | CanSat = 6; |
Sigma884 | 0:a38bf4917128 | 362 | } |
Sigma884 | 0:a38bf4917128 | 363 | |
Sigma884 | 0:a38bf4917128 | 364 | ////////////////////////////////////////////////////////////////////////天気を見て変える |
Sigma884 | 0:a38bf4917128 | 365 | if(red_per >= 1){ |
Sigma884 | 0:a38bf4917128 | 366 | MotorL.turn_a(0.6f); |
Sigma884 | 0:a38bf4917128 | 367 | MotorR.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 368 | if(red_per >= 40){ |
Sigma884 | 0:a38bf4917128 | 369 | CanSat = 6; |
Sigma884 | 0:a38bf4917128 | 370 | } |
Sigma884 | 0:a38bf4917128 | 371 | else if(red_per < 3){ |
Sigma884 | 0:a38bf4917128 | 372 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 373 | } |
Sigma884 | 0:a38bf4917128 | 374 | else if(red_per < 8){ |
Sigma884 | 0:a38bf4917128 | 375 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 376 | } |
Sigma884 | 0:a38bf4917128 | 377 | else if(red_per < 13){ |
Sigma884 | 0:a38bf4917128 | 378 | wait(1.5f); |
Sigma884 | 0:a38bf4917128 | 379 | } |
Sigma884 | 0:a38bf4917128 | 380 | else{ |
Sigma884 | 0:a38bf4917128 | 381 | wait(2.0f); |
Sigma884 | 0:a38bf4917128 | 382 | } |
Sigma884 | 0:a38bf4917128 | 383 | } |
Sigma884 | 0:a38bf4917128 | 384 | else{ |
Sigma884 | 0:a38bf4917128 | 385 | MotorL.turn_a(0.3f); |
Sigma884 | 0:a38bf4917128 | 386 | MotorR.turn_a(0.6f); |
Sigma884 | 0:a38bf4917128 | 387 | } |
Sigma884 | 0:a38bf4917128 | 388 | |
Sigma884 | 0:a38bf4917128 | 389 | /* |
Sigma884 | 0:a38bf4917128 | 390 | if((red_per >= 5 && red_center_x != -1) || red_per >= 10){ |
Sigma884 | 0:a38bf4917128 | 391 | if(red_per >= 40){ //天候次第 |
Sigma884 | 0:a38bf4917128 | 392 | CanSat = 6; |
Sigma884 | 0:a38bf4917128 | 393 | } |
Sigma884 | 0:a38bf4917128 | 394 | */ |
Sigma884 | 0:a38bf4917128 | 395 | /* |
Sigma884 | 0:a38bf4917128 | 396 | MotorL.turn_a(0.4f); //グラウンド状況次第 |
Sigma884 | 0:a38bf4917128 | 397 | MotorR.turn_a(0.2f); |
Sigma884 | 0:a38bf4917128 | 398 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 399 | */ |
Sigma884 | 0:a38bf4917128 | 400 | /* |
Sigma884 | 0:a38bf4917128 | 401 | MotorL.turn_a(0.6f); |
Sigma884 | 0:a38bf4917128 | 402 | MotorR.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 403 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 404 | } |
Sigma884 | 0:a38bf4917128 | 405 | else{ |
Sigma884 | 0:a38bf4917128 | 406 | MotorL.turn_a(0.3f); |
Sigma884 | 0:a38bf4917128 | 407 | MotorR.turn_a(0.6f); |
Sigma884 | 0:a38bf4917128 | 408 | } |
Sigma884 | 0:a38bf4917128 | 409 | */ |
Sigma884 | 0:a38bf4917128 | 410 | } |
Sigma884 | 0:a38bf4917128 | 411 | MotorL.stop(); |
Sigma884 | 0:a38bf4917128 | 412 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 413 | } |
Sigma884 | 0:a38bf4917128 | 414 | |
Sigma884 | 0:a38bf4917128 | 415 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 416 | Phase6 |
Sigma884 | 0:a38bf4917128 | 417 | 終了・回収待機中 |
Sigma884 | 0:a38bf4917128 | 418 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 419 | void phase6(){ |
Sigma884 | 0:a38bf4917128 | 420 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 421 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 422 | sendTick_RPi.detach(); |
Sigma884 | 0:a38bf4917128 | 423 | sendData(); |
Sigma884 | 0:a38bf4917128 | 424 | recData(); |
Sigma884 | 0:a38bf4917128 | 425 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 426 | sendTick_XB.attach(&sendData, 5.0f); |
Sigma884 | 0:a38bf4917128 | 427 | sendTick_SD.attach(&recData, 5.0f); |
Sigma884 | 0:a38bf4917128 | 428 | sendTick_RPi.attach(&sendRPi, 1.0f); |
Sigma884 | 0:a38bf4917128 | 429 | |
Sigma884 | 0:a38bf4917128 | 430 | Button.fall(&buttonPush); |
Sigma884 | 0:a38bf4917128 | 431 | |
Sigma884 | 0:a38bf4917128 | 432 | while(CanSat == 6){ |
Sigma884 | 0:a38bf4917128 | 433 | } |
Sigma884 | 0:a38bf4917128 | 434 | } |
Sigma884 | 0:a38bf4917128 | 435 | |
Sigma884 | 0:a38bf4917128 | 436 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 437 | 割り込み:Phase1 -> Phase2 |
Sigma884 | 0:a38bf4917128 | 438 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 439 | void flight(){ |
Sigma884 | 0:a38bf4917128 | 440 | if(!interrupt && !flight_yet && safety){ |
Sigma884 | 0:a38bf4917128 | 441 | CanSat = 2; |
Sigma884 | 0:a38bf4917128 | 442 | flight_yet = true; |
Sigma884 | 0:a38bf4917128 | 443 | } |
Sigma884 | 0:a38bf4917128 | 444 | } |
Sigma884 | 0:a38bf4917128 | 445 | |
Sigma884 | 0:a38bf4917128 | 446 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 447 | ゴール方向を計算 |
Sigma884 | 0:a38bf4917128 | 448 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 449 | int setRTG(){ |
Sigma884 | 0:a38bf4917128 | 450 | double target_rad0 = atan2(lat_target - gps_lat, lon_target - gps_lon) - atan2(gps_lat - lat_0, gps_lon - lon_0); |
Sigma884 | 0:a38bf4917128 | 451 | double target_rad1 = target_rad0 * 180 / atan(1.0) / 4; |
Sigma884 | 0:a38bf4917128 | 452 | int target_rad = (int)target_rad1; |
Sigma884 | 0:a38bf4917128 | 453 | if(target_rad > 180){ |
Sigma884 | 0:a38bf4917128 | 454 | return target_rad - 360; |
Sigma884 | 0:a38bf4917128 | 455 | } |
Sigma884 | 0:a38bf4917128 | 456 | else if(target_rad < -180){ |
Sigma884 | 0:a38bf4917128 | 457 | return target_rad + 360; |
Sigma884 | 0:a38bf4917128 | 458 | } |
Sigma884 | 0:a38bf4917128 | 459 | else{ |
Sigma884 | 0:a38bf4917128 | 460 | return target_rad; |
Sigma884 | 0:a38bf4917128 | 461 | } |
Sigma884 | 0:a38bf4917128 | 462 | } |
Sigma884 | 0:a38bf4917128 | 463 | |
Sigma884 | 0:a38bf4917128 | 464 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 465 | ゴールまでの距離を計算 |
Sigma884 | 0:a38bf4917128 | 466 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 467 | float setLTG(){ |
Sigma884 | 0:a38bf4917128 | 468 | float r_earth = 40 * pow(10.0f, 6); |
Sigma884 | 0:a38bf4917128 | 469 | float LTG_lat = r_earth / 360 * (lat_target - gps_lat); |
Sigma884 | 0:a38bf4917128 | 470 | float LTG_lon = r_earth * cos(lat_target / 180 * 3.141592654) / 360 * (lon_target - gps_lon); |
Sigma884 | 0:a38bf4917128 | 471 | |
Sigma884 | 0:a38bf4917128 | 472 | return sqrt(pow(LTG_lat, 2) + pow(LTG_lon, 2)); |
Sigma884 | 0:a38bf4917128 | 473 | } |
Sigma884 | 0:a38bf4917128 | 474 | |
Sigma884 | 0:a38bf4917128 | 475 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 476 | スタック脱出 |
Sigma884 | 0:a38bf4917128 | 477 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 478 | void escape(){ |
Sigma884 | 0:a38bf4917128 | 479 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 480 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 481 | sendTick_RPi.detach(); |
Sigma884 | 0:a38bf4917128 | 482 | sendData(); |
Sigma884 | 0:a38bf4917128 | 483 | recData(); |
Sigma884 | 0:a38bf4917128 | 484 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 485 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 486 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 487 | sendTick_RPi.attach(&sendRPi, 1.0f); |
Sigma884 | 0:a38bf4917128 | 488 | |
Sigma884 | 0:a38bf4917128 | 489 | Timer timer; |
Sigma884 | 0:a38bf4917128 | 490 | |
Sigma884 | 0:a38bf4917128 | 491 | esc = true; |
Sigma884 | 0:a38bf4917128 | 492 | MotorL.break_stop(); |
Sigma884 | 0:a38bf4917128 | 493 | MotorR.break_stop(); |
Sigma884 | 0:a38bf4917128 | 494 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 495 | |
Sigma884 | 0:a38bf4917128 | 496 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 497 | MotorL.turn_b(); |
Sigma884 | 0:a38bf4917128 | 498 | MotorR.turn_b(); |
Sigma884 | 0:a38bf4917128 | 499 | do{ |
Sigma884 | 0:a38bf4917128 | 500 | w_time = 4 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 501 | }while(w_time > 2); |
Sigma884 | 0:a38bf4917128 | 502 | |
Sigma884 | 0:a38bf4917128 | 503 | MotorL.stop(); |
Sigma884 | 0:a38bf4917128 | 504 | MotorR.turn_b(); |
Sigma884 | 0:a38bf4917128 | 505 | do{ |
Sigma884 | 0:a38bf4917128 | 506 | w_time = 4 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 507 | }while(w_time > 1); |
Sigma884 | 0:a38bf4917128 | 508 | |
Sigma884 | 0:a38bf4917128 | 509 | MotorL.turn_b(); |
Sigma884 | 0:a38bf4917128 | 510 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 511 | do{ |
Sigma884 | 0:a38bf4917128 | 512 | w_time = 4 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 513 | }while(w_time > 0); |
Sigma884 | 0:a38bf4917128 | 514 | |
Sigma884 | 0:a38bf4917128 | 515 | MotorL.stop(); |
Sigma884 | 0:a38bf4917128 | 516 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 517 | |
Sigma884 | 0:a38bf4917128 | 518 | esc = false; |
Sigma884 | 0:a38bf4917128 | 519 | } |
Sigma884 | 0:a38bf4917128 | 520 | |
Sigma884 | 0:a38bf4917128 | 521 | void escape2(){ |
Sigma884 | 0:a38bf4917128 | 522 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 523 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 524 | sendTick_RPi.detach(); |
Sigma884 | 0:a38bf4917128 | 525 | sendData(); |
Sigma884 | 0:a38bf4917128 | 526 | recData(); |
Sigma884 | 0:a38bf4917128 | 527 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 528 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 529 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 530 | sendTick_RPi.attach(&sendRPi, 1.0f); |
Sigma884 | 0:a38bf4917128 | 531 | |
Sigma884 | 0:a38bf4917128 | 532 | esc = true; |
Sigma884 | 0:a38bf4917128 | 533 | |
Sigma884 | 0:a38bf4917128 | 534 | MotorL.break_stop(); |
Sigma884 | 0:a38bf4917128 | 535 | MotorR.break_stop(); |
Sigma884 | 0:a38bf4917128 | 536 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 537 | MotorL.turn_b(); |
Sigma884 | 0:a38bf4917128 | 538 | MotorR.turn_b(); |
Sigma884 | 0:a38bf4917128 | 539 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 540 | MotorL.turn_b(); |
Sigma884 | 0:a38bf4917128 | 541 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 542 | wait(90.0 * 5 / 1000); |
Sigma884 | 0:a38bf4917128 | 543 | |
Sigma884 | 0:a38bf4917128 | 544 | esc = false; |
Sigma884 | 0:a38bf4917128 | 545 | } |
Sigma884 | 0:a38bf4917128 | 546 | |
Sigma884 | 0:a38bf4917128 | 547 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 548 | 旋回 |
Sigma884 | 0:a38bf4917128 | 549 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 550 | void activate(){ |
Sigma884 | 0:a38bf4917128 | 551 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 552 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 553 | sendTick_RPi.detach(); |
Sigma884 | 0:a38bf4917128 | 554 | sendData(); |
Sigma884 | 0:a38bf4917128 | 555 | recData(); |
Sigma884 | 0:a38bf4917128 | 556 | sendRPi(); |
Sigma884 | 0:a38bf4917128 | 557 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 558 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 559 | sendTick_RPi.attach(&sendRPi, 1.0f); |
Sigma884 | 0:a38bf4917128 | 560 | |
Sigma884 | 0:a38bf4917128 | 561 | if(rtg > 0){ |
Sigma884 | 0:a38bf4917128 | 562 | MotorL.stop(); |
Sigma884 | 0:a38bf4917128 | 563 | MotorR.turn_a(1.0f); |
Sigma884 | 0:a38bf4917128 | 564 | wait((float)rtg * 5 / 1000); |
Sigma884 | 0:a38bf4917128 | 565 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 566 | } |
Sigma884 | 0:a38bf4917128 | 567 | else{ |
Sigma884 | 0:a38bf4917128 | 568 | MotorL.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 569 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 570 | wait((float)rtg * -5 / 1000); |
Sigma884 | 0:a38bf4917128 | 571 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 572 | } |
Sigma884 | 0:a38bf4917128 | 573 | |
Sigma884 | 0:a38bf4917128 | 574 | MotorL.turn_a(0.8f); |
Sigma884 | 0:a38bf4917128 | 575 | MotorR.turn_a(1.0f); |
Sigma884 | 0:a38bf4917128 | 576 | } |
Sigma884 | 0:a38bf4917128 | 577 | |
Sigma884 | 0:a38bf4917128 | 578 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 579 | GPS座標読み取り |
Sigma884 | 0:a38bf4917128 | 580 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 581 | void readGPS(){ |
Sigma884 | 0:a38bf4917128 | 582 | double gps_lat_now = GPS.Latitude(); |
Sigma884 | 0:a38bf4917128 | 583 | double gps_lon_now = GPS.Longitude(); |
Sigma884 | 0:a38bf4917128 | 584 | if(gps_lat_now > 28.0f && gps_lat_now < 33.0f){ |
Sigma884 | 0:a38bf4917128 | 585 | if(gps_lon_now > 128.0f && gps_lon_now < 133.0f){ |
Sigma884 | 0:a38bf4917128 | 586 | gps_lat = gps_lat_now; |
Sigma884 | 0:a38bf4917128 | 587 | gps_lon = gps_lon_now; |
Sigma884 | 0:a38bf4917128 | 588 | raw_count ++; |
Sigma884 | 0:a38bf4917128 | 589 | if(raw_count == 4){ |
Sigma884 | 0:a38bf4917128 | 590 | raw_count = 0; |
Sigma884 | 0:a38bf4917128 | 591 | } |
Sigma884 | 0:a38bf4917128 | 592 | } |
Sigma884 | 0:a38bf4917128 | 593 | } |
Sigma884 | 0:a38bf4917128 | 594 | |
Sigma884 | 0:a38bf4917128 | 595 | gps_alt = GPS.Height(); |
Sigma884 | 0:a38bf4917128 | 596 | GPS.getUTC(utc); |
Sigma884 | 0:a38bf4917128 | 597 | utc[3] += 9; |
Sigma884 | 0:a38bf4917128 | 598 | if(utc[3] >= 24){ |
Sigma884 | 0:a38bf4917128 | 599 | utc[3] -= 24; |
Sigma884 | 0:a38bf4917128 | 600 | } |
Sigma884 | 0:a38bf4917128 | 601 | gps_knot = GPS.Knot(); |
Sigma884 | 0:a38bf4917128 | 602 | gps_deg = GPS.Degree(); |
Sigma884 | 0:a38bf4917128 | 603 | gps_sat = GPS.Number(); |
Sigma884 | 0:a38bf4917128 | 604 | |
Sigma884 | 0:a38bf4917128 | 605 | ltg = setLTG(); |
Sigma884 | 0:a38bf4917128 | 606 | |
Sigma884 | 0:a38bf4917128 | 607 | if(CanSat == 1 && flag_ave){ //待機モード時のみ |
Sigma884 | 0:a38bf4917128 | 608 | GPSAverage(); |
Sigma884 | 0:a38bf4917128 | 609 | } |
Sigma884 | 0:a38bf4917128 | 610 | |
Sigma884 | 0:a38bf4917128 | 611 | if(CanSat == 4 && !esc){ //走行モード時のみ |
Sigma884 | 0:a38bf4917128 | 612 | rtg = setRTG(); |
Sigma884 | 0:a38bf4917128 | 613 | if(raw_count == 0){//raw_count == 4でリセットだから1秒ごと |
Sigma884 | 0:a38bf4917128 | 614 | reset_count ++; |
Sigma884 | 0:a38bf4917128 | 615 | if(ltg < 15){ |
Sigma884 | 0:a38bf4917128 | 616 | w_time = 3 - reset_count; |
Sigma884 | 0:a38bf4917128 | 617 | } |
Sigma884 | 0:a38bf4917128 | 618 | else{ |
Sigma884 | 0:a38bf4917128 | 619 | w_time = 5 - reset_count; |
Sigma884 | 0:a38bf4917128 | 620 | } |
Sigma884 | 0:a38bf4917128 | 621 | } |
Sigma884 | 0:a38bf4917128 | 622 | } |
Sigma884 | 0:a38bf4917128 | 623 | } |
Sigma884 | 0:a38bf4917128 | 624 | |
Sigma884 | 0:a38bf4917128 | 625 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 626 | センサー読み取り |
Sigma884 | 0:a38bf4917128 | 627 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 628 | void readSensor(){ |
Sigma884 | 0:a38bf4917128 | 629 | float pres_now = LPS25H.getPres(); |
Sigma884 | 0:a38bf4917128 | 630 | if(pres_now > 500 && pres_now < 1500){ |
Sigma884 | 0:a38bf4917128 | 631 | pres = pres_now; |
Sigma884 | 0:a38bf4917128 | 632 | temp = LPS25H.getTemp(); |
Sigma884 | 0:a38bf4917128 | 633 | alt = LPS25H.getAlt(pres_0, temp_0); |
Sigma884 | 0:a38bf4917128 | 634 | } |
Sigma884 | 0:a38bf4917128 | 635 | |
Sigma884 | 0:a38bf4917128 | 636 | if(CanSat == 2){ |
Sigma884 | 0:a38bf4917128 | 637 | if(alt < 5.0f && alt > -10.0f){ |
Sigma884 | 0:a38bf4917128 | 638 | sep_count ++; |
Sigma884 | 0:a38bf4917128 | 639 | } |
Sigma884 | 0:a38bf4917128 | 640 | } |
Sigma884 | 0:a38bf4917128 | 641 | |
Sigma884 | 0:a38bf4917128 | 642 | ADXL375.getOutput(acc); |
Sigma884 | 0:a38bf4917128 | 643 | gravity = sqrt(pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2)); |
Sigma884 | 0:a38bf4917128 | 644 | |
Sigma884 | 0:a38bf4917128 | 645 | vin = Vinpin.read() * 8.4; |
Sigma884 | 0:a38bf4917128 | 646 | } |
Sigma884 | 0:a38bf4917128 | 647 | |
Sigma884 | 0:a38bf4917128 | 648 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 649 | 0m地点での気圧・温度のリセット |
Sigma884 | 0:a38bf4917128 | 650 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 651 | void resetPT(){ |
Sigma884 | 0:a38bf4917128 | 652 | pres_0 = pres; |
Sigma884 | 0:a38bf4917128 | 653 | temp_0 = temp; |
Sigma884 | 0:a38bf4917128 | 654 | |
Sigma884 | 0:a38bf4917128 | 655 | safety = true; |
Sigma884 | 0:a38bf4917128 | 656 | |
Sigma884 | 0:a38bf4917128 | 657 | save = !save; |
Sigma884 | 0:a38bf4917128 | 658 | if(save){ |
Sigma884 | 0:a38bf4917128 | 659 | startRec(); |
Sigma884 | 0:a38bf4917128 | 660 | } |
Sigma884 | 0:a38bf4917128 | 661 | else{ |
Sigma884 | 0:a38bf4917128 | 662 | stopRec(); |
Sigma884 | 0:a38bf4917128 | 663 | } |
Sigma884 | 0:a38bf4917128 | 664 | } |
Sigma884 | 0:a38bf4917128 | 665 | |
Sigma884 | 0:a38bf4917128 | 666 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 667 | RPiからのデータ読み取り |
Sigma884 | 0:a38bf4917128 | 668 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 669 | void readRPi(){ |
Sigma884 | 0:a38bf4917128 | 670 | RPi_read[RPi_pt] = RPi.getc(); |
Sigma884 | 0:a38bf4917128 | 671 | RPi_pt ++; |
Sigma884 | 0:a38bf4917128 | 672 | |
Sigma884 | 0:a38bf4917128 | 673 | if(RPi_read[RPi_pt - 1] == '_'){ |
Sigma884 | 0:a38bf4917128 | 674 | if(RPi_pt > 3){ |
Sigma884 | 0:a38bf4917128 | 675 | RPi_read[RPi_pt - 1] = '\0'; |
Sigma884 | 0:a38bf4917128 | 676 | readCommand(); |
Sigma884 | 0:a38bf4917128 | 677 | } |
Sigma884 | 0:a38bf4917128 | 678 | RPi_pt = 0; |
Sigma884 | 0:a38bf4917128 | 679 | } |
Sigma884 | 0:a38bf4917128 | 680 | } |
Sigma884 | 0:a38bf4917128 | 681 | |
Sigma884 | 0:a38bf4917128 | 682 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 683 | RPiにデータ送信 |
Sigma884 | 0:a38bf4917128 | 684 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 685 | void sendRPi(){ |
Sigma884 | 0:a38bf4917128 | 686 | RPi.printf("%d", CanSat); |
Sigma884 | 0:a38bf4917128 | 687 | } |
Sigma884 | 0:a38bf4917128 | 688 | |
Sigma884 | 0:a38bf4917128 | 689 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 690 | RPiコマンド解析 |
Sigma884 | 0:a38bf4917128 | 691 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 692 | void readCommand(){ |
Sigma884 | 0:a38bf4917128 | 693 | char command[128]; |
Sigma884 | 0:a38bf4917128 | 694 | strcpy(command, RPi_read); |
Sigma884 | 0:a38bf4917128 | 695 | sscanf(command, "%d,%d,%d,%d,%d,%d,%d", &red_per, |
Sigma884 | 0:a38bf4917128 | 696 | &red_center_x, &red_center_y, |
Sigma884 | 0:a38bf4917128 | 697 | &red_moment_x, &red_moment_y, |
Sigma884 | 0:a38bf4917128 | 698 | &yel_per, &laser); |
Sigma884 | 0:a38bf4917128 | 699 | } |
Sigma884 | 0:a38bf4917128 | 700 | |
Sigma884 | 0:a38bf4917128 | 701 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 702 | 割り込み:アクチュエータチェック |
Sigma884 | 0:a38bf4917128 | 703 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 704 | void ConnectCheck(){ |
Sigma884 | 0:a38bf4917128 | 705 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 706 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 707 | sendTick_RPi.detach(); |
Sigma884 | 0:a38bf4917128 | 708 | |
Sigma884 | 0:a38bf4917128 | 709 | FlightPin.rise(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 710 | Button.fall(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 711 | Button.rise(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 712 | GPSAve.fall(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 713 | GPSAve.rise(&stopInterruptIn); |
Sigma884 | 0:a38bf4917128 | 714 | |
Sigma884 | 0:a38bf4917128 | 715 | Timer timer; |
Sigma884 | 0:a38bf4917128 | 716 | |
Sigma884 | 0:a38bf4917128 | 717 | pc.printf("\r\nTEST MODE\r\n"); |
Sigma884 | 0:a38bf4917128 | 718 | XB.printf("\r\nTEST MODE\r\n"); |
Sigma884 | 0:a38bf4917128 | 719 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 720 | |
Sigma884 | 0:a38bf4917128 | 721 | MotorL.turn_a(); |
Sigma884 | 0:a38bf4917128 | 722 | pc.printf("Motor Left : FRONT\r\n"); |
Sigma884 | 0:a38bf4917128 | 723 | XB.printf("Motor Left : FRONT\r\n"); |
Sigma884 | 0:a38bf4917128 | 724 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 725 | MotorL.turn_b(); |
Sigma884 | 0:a38bf4917128 | 726 | pc.printf("Motor Left : BACK\r\n"); |
Sigma884 | 0:a38bf4917128 | 727 | XB.printf("Motor Left : BACK\r\n"); |
Sigma884 | 0:a38bf4917128 | 728 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 729 | MotorL.stop(); |
Sigma884 | 0:a38bf4917128 | 730 | |
Sigma884 | 0:a38bf4917128 | 731 | MotorR.turn_a(); |
Sigma884 | 0:a38bf4917128 | 732 | pc.printf("Motor Right : FRONT\r\n"); |
Sigma884 | 0:a38bf4917128 | 733 | XB.printf("Motor Right : FRONT\r\n"); |
Sigma884 | 0:a38bf4917128 | 734 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 735 | MotorR.turn_b(); |
Sigma884 | 0:a38bf4917128 | 736 | pc.printf("Motor Right : BACK\r\n"); |
Sigma884 | 0:a38bf4917128 | 737 | XB.printf("Motor Right : BACK\r\n"); |
Sigma884 | 0:a38bf4917128 | 738 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 739 | MotorR.stop(); |
Sigma884 | 0:a38bf4917128 | 740 | |
Sigma884 | 0:a38bf4917128 | 741 | Nichrome1.fire(3.0f); |
Sigma884 | 0:a38bf4917128 | 742 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 743 | do{ |
Sigma884 | 0:a38bf4917128 | 744 | w_time = 6 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 745 | pc.printf("Nichrome 1 : %d %d\r", w_time, Nichrome1.status); |
Sigma884 | 0:a38bf4917128 | 746 | XB.printf("Nichrome 1 : %d %d\r", w_time, Nichrome1.status); |
Sigma884 | 0:a38bf4917128 | 747 | if(w_time == 3){ |
Sigma884 | 0:a38bf4917128 | 748 | Nichrome1.fire_off(); |
Sigma884 | 0:a38bf4917128 | 749 | } |
Sigma884 | 0:a38bf4917128 | 750 | wait(0.1f); |
Sigma884 | 0:a38bf4917128 | 751 | }while(w_time > 0); |
Sigma884 | 0:a38bf4917128 | 752 | timer.stop(); |
Sigma884 | 0:a38bf4917128 | 753 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 754 | pc.printf("\n"); |
Sigma884 | 0:a38bf4917128 | 755 | XB.printf("\n"); |
Sigma884 | 0:a38bf4917128 | 756 | |
Sigma884 | 0:a38bf4917128 | 757 | Nichrome2.fire(3.0f); |
Sigma884 | 0:a38bf4917128 | 758 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 759 | do{ |
Sigma884 | 0:a38bf4917128 | 760 | w_time = 6 - timer.read(); |
Sigma884 | 0:a38bf4917128 | 761 | pc.printf("Nichrome 2 : %d %d\r", w_time, Nichrome2.status); |
Sigma884 | 0:a38bf4917128 | 762 | XB.printf("Nichrome 2 : %d %d\r", w_time, Nichrome2.status); |
Sigma884 | 0:a38bf4917128 | 763 | if(w_time == 3){ |
Sigma884 | 0:a38bf4917128 | 764 | Nichrome2.fire_off(); |
Sigma884 | 0:a38bf4917128 | 765 | } |
Sigma884 | 0:a38bf4917128 | 766 | wait(0.1f); |
Sigma884 | 0:a38bf4917128 | 767 | }while(w_time > 0); |
Sigma884 | 0:a38bf4917128 | 768 | timer.stop(); |
Sigma884 | 0:a38bf4917128 | 769 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 770 | pc.printf("\n"); |
Sigma884 | 0:a38bf4917128 | 771 | XB.printf("\n"); |
Sigma884 | 0:a38bf4917128 | 772 | |
Sigma884 | 0:a38bf4917128 | 773 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 774 | |
Sigma884 | 0:a38bf4917128 | 775 | pc.printf("Test Finish!\r\n\n"); |
Sigma884 | 0:a38bf4917128 | 776 | XB.printf("Test Finish!\r\n\n"); |
Sigma884 | 0:a38bf4917128 | 777 | |
Sigma884 | 0:a38bf4917128 | 778 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 779 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 780 | sendTick_RPi.attach(&sendRPi, 1.0f); |
Sigma884 | 0:a38bf4917128 | 781 | |
Sigma884 | 0:a38bf4917128 | 782 | FlightPin.rise(&flight); |
Sigma884 | 0:a38bf4917128 | 783 | Button.fall(&buttonPush); |
Sigma884 | 0:a38bf4917128 | 784 | Button.rise(&buttonRelease); |
Sigma884 | 0:a38bf4917128 | 785 | GPSAve.fall(&startGPSAve); |
Sigma884 | 0:a38bf4917128 | 786 | GPSAve.rise(&stopGPSAve); |
Sigma884 | 0:a38bf4917128 | 787 | } |
Sigma884 | 0:a38bf4917128 | 788 | |
Sigma884 | 0:a38bf4917128 | 789 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 790 | 割り込み:GPS平均計算開始 |
Sigma884 | 0:a38bf4917128 | 791 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 792 | void startGPSAve(){ |
Sigma884 | 0:a38bf4917128 | 793 | if(!interrupt){ |
Sigma884 | 0:a38bf4917128 | 794 | sendTick_XB.detach(); |
Sigma884 | 0:a38bf4917128 | 795 | sendTick_SD.detach(); |
Sigma884 | 0:a38bf4917128 | 796 | |
Sigma884 | 0:a38bf4917128 | 797 | flag_ave = true; |
Sigma884 | 0:a38bf4917128 | 798 | lat_sum = 0; |
Sigma884 | 0:a38bf4917128 | 799 | lon_sum = 0; |
Sigma884 | 0:a38bf4917128 | 800 | csum = 0; |
Sigma884 | 0:a38bf4917128 | 801 | |
Sigma884 | 0:a38bf4917128 | 802 | interrupt = true; |
Sigma884 | 0:a38bf4917128 | 803 | } |
Sigma884 | 0:a38bf4917128 | 804 | } |
Sigma884 | 0:a38bf4917128 | 805 | |
Sigma884 | 0:a38bf4917128 | 806 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 807 | 割り込み:GPS平均計算終了 |
Sigma884 | 0:a38bf4917128 | 808 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 809 | void stopGPSAve(){ |
Sigma884 | 0:a38bf4917128 | 810 | if(interrupt){ |
Sigma884 | 0:a38bf4917128 | 811 | flag_ave = false; |
Sigma884 | 0:a38bf4917128 | 812 | |
Sigma884 | 0:a38bf4917128 | 813 | pc.printf("GPS Average\r\nCount : %.0lf\r\nLAT : %lf(N)\r\nLON : %lf(E)\r\n", csum, lat_ave, lon_ave); |
Sigma884 | 0:a38bf4917128 | 814 | XB.printf("GPS Average\r\nCount : %.0lf\r\nLAT : %lf(N)\r\nLON : %lf(E)\r\n", csum, lat_ave, lon_ave); |
Sigma884 | 0:a38bf4917128 | 815 | |
Sigma884 | 0:a38bf4917128 | 816 | wait(5.0f); |
Sigma884 | 0:a38bf4917128 | 817 | |
Sigma884 | 0:a38bf4917128 | 818 | sendTick_XB.attach(&sendData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 819 | sendTick_SD.attach(&recData, 1.0f); |
Sigma884 | 0:a38bf4917128 | 820 | |
Sigma884 | 0:a38bf4917128 | 821 | interrupt = false; |
Sigma884 | 0:a38bf4917128 | 822 | } |
Sigma884 | 0:a38bf4917128 | 823 | } |
Sigma884 | 0:a38bf4917128 | 824 | |
Sigma884 | 0:a38bf4917128 | 825 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 826 | GPS平均計算 |
Sigma884 | 0:a38bf4917128 | 827 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 828 | void GPSAverage(){ |
Sigma884 | 0:a38bf4917128 | 829 | lat_sum += gps_lat; |
Sigma884 | 0:a38bf4917128 | 830 | lon_sum += gps_lon; |
Sigma884 | 0:a38bf4917128 | 831 | csum ++; |
Sigma884 | 0:a38bf4917128 | 832 | lat_ave = lat_sum / csum; |
Sigma884 | 0:a38bf4917128 | 833 | lon_ave = lon_sum / csum; |
Sigma884 | 0:a38bf4917128 | 834 | |
Sigma884 | 0:a38bf4917128 | 835 | pc.printf("GPS AVE(%.0lf) : %lf(N) , %lf(E)\r\n", csum, lat_ave, lon_ave); |
Sigma884 | 0:a38bf4917128 | 836 | XB.printf("GPS AVE(%.0lf) : %lf(N) , %lf(E)\r\n", csum, lat_ave, lon_ave); |
Sigma884 | 0:a38bf4917128 | 837 | } |
Sigma884 | 0:a38bf4917128 | 838 | |
Sigma884 | 0:a38bf4917128 | 839 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 840 | ピン割り込み停止 |
Sigma884 | 0:a38bf4917128 | 841 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 842 | void stopInterruptIn(){ |
Sigma884 | 0:a38bf4917128 | 843 | pc.printf("Stopped Interrupt\r\n"); |
Sigma884 | 0:a38bf4917128 | 844 | } |
Sigma884 | 0:a38bf4917128 | 845 | |
Sigma884 | 0:a38bf4917128 | 846 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 847 | 割り込み:ボタンを押したとき |
Sigma884 | 0:a38bf4917128 | 848 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 849 | void buttonPush(){ |
Sigma884 | 0:a38bf4917128 | 850 | if(!interrupt && !button_push){ |
Sigma884 | 0:a38bf4917128 | 851 | timer.start(); |
Sigma884 | 0:a38bf4917128 | 852 | button_push = true; |
Sigma884 | 0:a38bf4917128 | 853 | interrupt = true; |
Sigma884 | 0:a38bf4917128 | 854 | } |
Sigma884 | 0:a38bf4917128 | 855 | } |
Sigma884 | 0:a38bf4917128 | 856 | |
Sigma884 | 0:a38bf4917128 | 857 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 858 | 割り込み:ボタンを離したとき |
Sigma884 | 0:a38bf4917128 | 859 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 860 | void buttonRelease(){ |
Sigma884 | 0:a38bf4917128 | 861 | if(interrupt && button_push){ |
Sigma884 | 0:a38bf4917128 | 862 | int time_button; |
Sigma884 | 0:a38bf4917128 | 863 | button_push = false; |
Sigma884 | 0:a38bf4917128 | 864 | time_button = timer.read(); |
Sigma884 | 0:a38bf4917128 | 865 | timer.stop(); |
Sigma884 | 0:a38bf4917128 | 866 | timer.reset(); |
Sigma884 | 0:a38bf4917128 | 867 | if(time_button >= 3){ |
Sigma884 | 0:a38bf4917128 | 868 | ConnectCheck(); |
Sigma884 | 0:a38bf4917128 | 869 | } |
Sigma884 | 0:a38bf4917128 | 870 | else{ |
Sigma884 | 0:a38bf4917128 | 871 | resetPT(); |
Sigma884 | 0:a38bf4917128 | 872 | } |
Sigma884 | 0:a38bf4917128 | 873 | interrupt = false; |
Sigma884 | 0:a38bf4917128 | 874 | } |
Sigma884 | 0:a38bf4917128 | 875 | } |
Sigma884 | 0:a38bf4917128 | 876 | |
Sigma884 | 0:a38bf4917128 | 877 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 878 | PCでデータ表示 |
Sigma884 | 0:a38bf4917128 | 879 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 880 | void printData(){ |
Sigma884 | 0:a38bf4917128 | 881 | for(int c1 = 0; c1 < 20; c1 ++){ |
Sigma884 | 0:a38bf4917128 | 882 | pc.printf("*"); |
Sigma884 | 0:a38bf4917128 | 883 | } |
Sigma884 | 0:a38bf4917128 | 884 | pc.printf("\r\n"); |
Sigma884 | 0:a38bf4917128 | 885 | pc.printf("MODE -> "); |
Sigma884 | 0:a38bf4917128 | 886 | switch(CanSat){ |
Sigma884 | 0:a38bf4917128 | 887 | case 0: |
Sigma884 | 0:a38bf4917128 | 888 | pc.printf("Setting\r\n"); |
Sigma884 | 0:a38bf4917128 | 889 | break; |
Sigma884 | 0:a38bf4917128 | 890 | case 1: |
Sigma884 | 0:a38bf4917128 | 891 | pc.printf("Wait\r\n"); |
Sigma884 | 0:a38bf4917128 | 892 | break; |
Sigma884 | 0:a38bf4917128 | 893 | case 2: |
Sigma884 | 0:a38bf4917128 | 894 | pc.printf("Descend\r\n"); |
Sigma884 | 0:a38bf4917128 | 895 | break; |
Sigma884 | 0:a38bf4917128 | 896 | case 3: |
Sigma884 | 0:a38bf4917128 | 897 | pc.printf("Separate\r\n"); |
Sigma884 | 0:a38bf4917128 | 898 | break; |
Sigma884 | 0:a38bf4917128 | 899 | case 4: |
Sigma884 | 0:a38bf4917128 | 900 | pc.printf("GPS\r\n"); |
Sigma884 | 0:a38bf4917128 | 901 | break; |
Sigma884 | 0:a38bf4917128 | 902 | case 5: |
Sigma884 | 0:a38bf4917128 | 903 | pc.printf("Camera\r\n"); |
Sigma884 | 0:a38bf4917128 | 904 | break; |
Sigma884 | 0:a38bf4917128 | 905 | case 6: |
Sigma884 | 0:a38bf4917128 | 906 | pc.printf("Finish\r\n"); |
Sigma884 | 0:a38bf4917128 | 907 | } |
Sigma884 | 0:a38bf4917128 | 908 | pc.printf("GPS -> %3.7f(N), %3.7f(E), %d\r\n", gps_lat, gps_lon, gps_sat); |
Sigma884 | 0:a38bf4917128 | 909 | pc.printf("TIME -> %d:%d:%02.2f\r\n", (int)utc[3], (int)utc[4], utc[5]); |
Sigma884 | 0:a38bf4917128 | 910 | pc.printf("LPS25H -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt); |
Sigma884 | 0:a38bf4917128 | 911 | pc.printf("W_TIME -> %d[s]\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 912 | pc.printf("Nichrome -> %d, %d\r\n", Nichrome1.status, Nichrome2.status); |
Sigma884 | 0:a38bf4917128 | 913 | wait(0.05f); |
Sigma884 | 0:a38bf4917128 | 914 | pc.printf("RTG -> %d[degree]\r\n", rtg); |
Sigma884 | 0:a38bf4917128 | 915 | pc.printf("LTG -> %.2lf[m]\r\n", ltg); |
Sigma884 | 0:a38bf4917128 | 916 | pc.printf("Motor -> %.2f(L), %.2f(R)\r\n", MotorL.status, MotorR.status); |
Sigma884 | 0:a38bf4917128 | 917 | pc.printf("Stack -> %d\r\n", esc); |
Sigma884 | 0:a38bf4917128 | 918 | pc.printf("Save -> %d\r\n", save); |
Sigma884 | 0:a38bf4917128 | 919 | pc.printf("Battery -> %.2f\r\n", vin); |
Sigma884 | 0:a38bf4917128 | 920 | pc.printf("Red PER -> %d\r\n", red_per); |
Sigma884 | 0:a38bf4917128 | 921 | pc.printf("Red TRI -> %d, %d\r\n", red_center_x, red_center_y); |
Sigma884 | 0:a38bf4917128 | 922 | pc.printf("Red MOM -> %d, %d\r\n", red_moment_x, red_moment_y); |
Sigma884 | 0:a38bf4917128 | 923 | pc.printf("Yellow -> %d\r\n", yel_per); |
Sigma884 | 0:a38bf4917128 | 924 | pc.printf("Laser -> %d\r\n\n\n", laser); |
Sigma884 | 0:a38bf4917128 | 925 | } |
Sigma884 | 0:a38bf4917128 | 926 | |
Sigma884 | 0:a38bf4917128 | 927 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 928 | 地上局へデータ送信 |
Sigma884 | 0:a38bf4917128 | 929 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 930 | void sendData(){ |
Sigma884 | 0:a38bf4917128 | 931 | if(CanSat == 2 || CanSat == 3){ |
Sigma884 | 0:a38bf4917128 | 932 | XB.printf("%d[MODE],\t", CanSat); |
Sigma884 | 0:a38bf4917128 | 933 | XB.printf("%d[s],\t%.4f[hPa],\t%.2f[m],\t", w_time, pres, alt); |
Sigma884 | 0:a38bf4917128 | 934 | XB.printf("%d,\t%d %d\r\n", sep_count, Nichrome1.status, Nichrome2.status); |
Sigma884 | 0:a38bf4917128 | 935 | } |
Sigma884 | 0:a38bf4917128 | 936 | else{ |
Sigma884 | 0:a38bf4917128 | 937 | for(int c1 = 0; c1 < 20; c1 ++){ |
Sigma884 | 0:a38bf4917128 | 938 | XB.printf("*"); |
Sigma884 | 0:a38bf4917128 | 939 | } |
Sigma884 | 0:a38bf4917128 | 940 | XB.printf("\r\n"); |
Sigma884 | 0:a38bf4917128 | 941 | XB.printf("MODE -> "); |
Sigma884 | 0:a38bf4917128 | 942 | switch(CanSat){ |
Sigma884 | 0:a38bf4917128 | 943 | case 0: |
Sigma884 | 0:a38bf4917128 | 944 | XB.printf("Setting\r\n"); |
Sigma884 | 0:a38bf4917128 | 945 | break; |
Sigma884 | 0:a38bf4917128 | 946 | case 1: |
Sigma884 | 0:a38bf4917128 | 947 | XB.printf("Wait\r\n"); |
Sigma884 | 0:a38bf4917128 | 948 | break; |
Sigma884 | 0:a38bf4917128 | 949 | case 2: |
Sigma884 | 0:a38bf4917128 | 950 | XB.printf("Descend\r\n"); |
Sigma884 | 0:a38bf4917128 | 951 | break; |
Sigma884 | 0:a38bf4917128 | 952 | case 3: |
Sigma884 | 0:a38bf4917128 | 953 | XB.printf("Separate\r\n"); |
Sigma884 | 0:a38bf4917128 | 954 | break; |
Sigma884 | 0:a38bf4917128 | 955 | case 4: |
Sigma884 | 0:a38bf4917128 | 956 | XB.printf("GPS\r\n"); |
Sigma884 | 0:a38bf4917128 | 957 | break; |
Sigma884 | 0:a38bf4917128 | 958 | case 5: |
Sigma884 | 0:a38bf4917128 | 959 | XB.printf("Camera\r\n"); |
Sigma884 | 0:a38bf4917128 | 960 | break; |
Sigma884 | 0:a38bf4917128 | 961 | case 6: |
Sigma884 | 0:a38bf4917128 | 962 | XB.printf("Finish\r\n"); |
Sigma884 | 0:a38bf4917128 | 963 | } |
Sigma884 | 0:a38bf4917128 | 964 | XB.printf("GPS -> %3.7f(N), %3.7f(E), %d\r\n", gps_lat, gps_lon, gps_sat); |
Sigma884 | 0:a38bf4917128 | 965 | XB.printf("TIME -> %d:%d:%02.2f\r\n", (int)utc[3], (int)utc[4], utc[5]); |
Sigma884 | 0:a38bf4917128 | 966 | XB.printf("LPS25H -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt); |
Sigma884 | 0:a38bf4917128 | 967 | XB.printf("W_TIME -> %d[s]\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 968 | XB.printf("Nichrome -> %d, %d\r\n", Nichrome1.status, Nichrome2.status); |
Sigma884 | 0:a38bf4917128 | 969 | wait(0.05f); |
Sigma884 | 0:a38bf4917128 | 970 | XB.printf("RTG -> %d[degree]\r\n", rtg); |
Sigma884 | 0:a38bf4917128 | 971 | XB.printf("LTG -> %.2lf[m]\r\n", ltg); |
Sigma884 | 0:a38bf4917128 | 972 | XB.printf("Motor -> %.2f(L), %.2f(R)\r\n", MotorL.status, MotorR.status); |
Sigma884 | 0:a38bf4917128 | 973 | XB.printf("Stack -> %d\r\n", esc); |
Sigma884 | 0:a38bf4917128 | 974 | XB.printf("Save -> %d\r\n", save); |
Sigma884 | 0:a38bf4917128 | 975 | XB.printf("Battery -> %.2f\r\n", vin); |
Sigma884 | 0:a38bf4917128 | 976 | wait(0.05f); |
Sigma884 | 0:a38bf4917128 | 977 | XB.printf("Red PER -> %d\r\n", red_per); |
Sigma884 | 0:a38bf4917128 | 978 | XB.printf("Red TRI -> %d, %d\r\n", red_center_x, red_center_y); |
Sigma884 | 0:a38bf4917128 | 979 | XB.printf("Red MOM -> %d, %d\r\n", red_moment_x, red_moment_y); |
Sigma884 | 0:a38bf4917128 | 980 | XB.printf("Yellow -> %d\r\n", yel_per); |
Sigma884 | 0:a38bf4917128 | 981 | XB.printf("Laser -> %d\r\n\n\n", laser); |
Sigma884 | 0:a38bf4917128 | 982 | } |
Sigma884 | 0:a38bf4917128 | 983 | } |
Sigma884 | 0:a38bf4917128 | 984 | |
Sigma884 | 0:a38bf4917128 | 985 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 986 | ログ保存 |
Sigma884 | 0:a38bf4917128 | 987 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 988 | void recData(){ |
Sigma884 | 0:a38bf4917128 | 989 | if(save){ |
Sigma884 | 0:a38bf4917128 | 990 | //時間情報 |
Sigma884 | 0:a38bf4917128 | 991 | fprintf(fp, "%d/%d/%d,", (int)utc[0], (int)utc[1], (int)utc[2]); //日付 |
Sigma884 | 0:a38bf4917128 | 992 | fprintf(fp, "%d:%02d:%02.2f,", (int)utc[3], (int)utc[4], utc[5]); //時間 |
Sigma884 | 0:a38bf4917128 | 993 | switch(CanSat){ //モード |
Sigma884 | 0:a38bf4917128 | 994 | case 0: |
Sigma884 | 0:a38bf4917128 | 995 | fprintf(fp, "Setting,"); |
Sigma884 | 0:a38bf4917128 | 996 | break; |
Sigma884 | 0:a38bf4917128 | 997 | case 1: |
Sigma884 | 0:a38bf4917128 | 998 | fprintf(fp, "Wait,"); |
Sigma884 | 0:a38bf4917128 | 999 | break; |
Sigma884 | 0:a38bf4917128 | 1000 | case 2: |
Sigma884 | 0:a38bf4917128 | 1001 | fprintf(fp, "Descend,"); |
Sigma884 | 0:a38bf4917128 | 1002 | break; |
Sigma884 | 0:a38bf4917128 | 1003 | case 3: |
Sigma884 | 0:a38bf4917128 | 1004 | fprintf(fp, "Separate,"); |
Sigma884 | 0:a38bf4917128 | 1005 | break; |
Sigma884 | 0:a38bf4917128 | 1006 | case 4: |
Sigma884 | 0:a38bf4917128 | 1007 | fprintf(fp, "GPS,"); |
Sigma884 | 0:a38bf4917128 | 1008 | break; |
Sigma884 | 0:a38bf4917128 | 1009 | case 5: |
Sigma884 | 0:a38bf4917128 | 1010 | fprintf(fp, "Camera,"); |
Sigma884 | 0:a38bf4917128 | 1011 | break; |
Sigma884 | 0:a38bf4917128 | 1012 | case 6: |
Sigma884 | 0:a38bf4917128 | 1013 | fprintf(fp, "Finish,"); |
Sigma884 | 0:a38bf4917128 | 1014 | } |
Sigma884 | 0:a38bf4917128 | 1015 | //センサ情報 |
Sigma884 | 0:a38bf4917128 | 1016 | fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標 |
Sigma884 | 0:a38bf4917128 | 1017 | fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度 |
Sigma884 | 0:a38bf4917128 | 1018 | fprintf(fp, "%d,", gps_sat); //GPS衛星数 |
Sigma884 | 0:a38bf4917128 | 1019 | fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt); //気圧センサ |
Sigma884 | 0:a38bf4917128 | 1020 | fprintf(fp, "%.2f,%.2f,%.2f,%.4f,", acc[0], acc[1], acc[2], gravity);//加速度センサ |
Sigma884 | 0:a38bf4917128 | 1021 | fprintf(fp, "%.2f,", vin); //電源電圧 |
Sigma884 | 0:a38bf4917128 | 1022 | //制御情報 |
Sigma884 | 0:a38bf4917128 | 1023 | fprintf(fp, "%d,", w_time); //待機時間 |
Sigma884 | 0:a38bf4917128 | 1024 | fprintf(fp, "%d,", rtg); //RadToGoal(ゴールの方向) |
Sigma884 | 0:a38bf4917128 | 1025 | fprintf(fp, "%.2lf,", ltg); //LengthToGal(ゴールまでの距離) |
Sigma884 | 0:a38bf4917128 | 1026 | //アクチュエータ情報 |
Sigma884 | 0:a38bf4917128 | 1027 | fprintf(fp, "%d,%d,", Nichrome1.status, Nichrome2.status); //ニクロム線 |
Sigma884 | 0:a38bf4917128 | 1028 | fprintf(fp, "%.2f,%.2f,", MotorL.status, MotorR.status); //モーター |
Sigma884 | 0:a38bf4917128 | 1029 | fprintf(fp, "%d,", esc); //スタックしているか |
Sigma884 | 0:a38bf4917128 | 1030 | //ラズパイ情報 |
Sigma884 | 0:a38bf4917128 | 1031 | fprintf(fp, "%d,", red_per); //赤色の割合 |
Sigma884 | 0:a38bf4917128 | 1032 | fprintf(fp, "%d,%d,", red_center_x, red_center_y); //赤色三角形重心の座標 |
Sigma884 | 0:a38bf4917128 | 1033 | fprintf(fp, "%d,%d,", red_moment_x, red_moment_y); //赤色重心の座標 |
Sigma884 | 0:a38bf4917128 | 1034 | fprintf(fp, "%d,", yel_per); |
Sigma884 | 0:a38bf4917128 | 1035 | fprintf(fp, "%d", laser); //距離センサ |
Sigma884 | 0:a38bf4917128 | 1036 | fprintf(fp, "\r\n"); |
Sigma884 | 0:a38bf4917128 | 1037 | } |
Sigma884 | 0:a38bf4917128 | 1038 | } |
Sigma884 | 0:a38bf4917128 | 1039 | |
Sigma884 | 0:a38bf4917128 | 1040 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 1041 | 記録開始 |
Sigma884 | 0:a38bf4917128 | 1042 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 1043 | void startRec(){ |
Sigma884 | 0:a38bf4917128 | 1044 | fp = fopen(file_name, "a"); |
Sigma884 | 0:a38bf4917128 | 1045 | pc.printf("START SAVING\r\n"); |
Sigma884 | 0:a38bf4917128 | 1046 | XB.printf("START SAVING\r\n"); |
Sigma884 | 0:a38bf4917128 | 1047 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 1048 | } |
Sigma884 | 0:a38bf4917128 | 1049 | |
Sigma884 | 0:a38bf4917128 | 1050 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 1051 | 記録終了 |
Sigma884 | 0:a38bf4917128 | 1052 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 1053 | void stopRec(){ |
Sigma884 | 0:a38bf4917128 | 1054 | fprintf(fp, "\r\n\n"); |
Sigma884 | 0:a38bf4917128 | 1055 | fclose(fp); |
Sigma884 | 0:a38bf4917128 | 1056 | pc.printf("STOP SAVING\r\n"); |
Sigma884 | 0:a38bf4917128 | 1057 | XB.printf("STOP SAVING\r\n"); |
Sigma884 | 0:a38bf4917128 | 1058 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 1059 | } |
Sigma884 | 0:a38bf4917128 | 1060 | |
Sigma884 | 0:a38bf4917128 | 1061 | /************************************************ |
Sigma884 | 0:a38bf4917128 | 1062 | セットアップ(最初に1回実行) |
Sigma884 | 0:a38bf4917128 | 1063 | ************************************************/ |
Sigma884 | 0:a38bf4917128 | 1064 | void setup(){ |
Sigma884 | 0:a38bf4917128 | 1065 | wait(0.5f); |
Sigma884 | 0:a38bf4917128 | 1066 | char setup_string[512]; |
Sigma884 | 0:a38bf4917128 | 1067 | char sprint_buff[64]; |
Sigma884 | 0:a38bf4917128 | 1068 | |
Sigma884 | 0:a38bf4917128 | 1069 | pc.printf("\r\n\nSetting Start\r\n"); |
Sigma884 | 0:a38bf4917128 | 1070 | XB.printf("\r\n\nSetting Start\r\n"); |
Sigma884 | 0:a38bf4917128 | 1071 | strcat(setup_string, "\r\n\nSetting Start\r\n"); |
Sigma884 | 0:a38bf4917128 | 1072 | |
Sigma884 | 0:a38bf4917128 | 1073 | //LPS25Hの設定 |
Sigma884 | 0:a38bf4917128 | 1074 | LPS25H.begin(25); |
Sigma884 | 0:a38bf4917128 | 1075 | LPS25H.setFIFO(16); |
Sigma884 | 0:a38bf4917128 | 1076 | if(LPS25H.whoAmI() == 1){ |
Sigma884 | 0:a38bf4917128 | 1077 | pc.printf("LPS25H : OK\r\n"); |
Sigma884 | 0:a38bf4917128 | 1078 | XB.printf("LPS25H : OK\r\n"); |
Sigma884 | 0:a38bf4917128 | 1079 | strcat(setup_string, "LPS25H : OK\r\n"); |
Sigma884 | 0:a38bf4917128 | 1080 | pres_0 = LPS25H.getPres(); |
Sigma884 | 0:a38bf4917128 | 1081 | temp_0 = LPS25H.getTemp(); |
Sigma884 | 0:a38bf4917128 | 1082 | } |
Sigma884 | 0:a38bf4917128 | 1083 | else{ |
Sigma884 | 0:a38bf4917128 | 1084 | pc.printf("LPS25H : NG...\r\n"); |
Sigma884 | 0:a38bf4917128 | 1085 | XB.printf("LPS25H : NG...\r\n"); |
Sigma884 | 0:a38bf4917128 | 1086 | strcat(setup_string, "LPS25H : NG...\r\n"); |
Sigma884 | 0:a38bf4917128 | 1087 | } |
Sigma884 | 0:a38bf4917128 | 1088 | |
Sigma884 | 0:a38bf4917128 | 1089 | //ADXL375の設定 |
Sigma884 | 0:a38bf4917128 | 1090 | ADXL375.setDataRate(ADXL375_3200HZ); |
Sigma884 | 0:a38bf4917128 | 1091 | if(ADXL375.whoAmI() == 1){ |
Sigma884 | 0:a38bf4917128 | 1092 | pc.printf("ADXL375 : OK\r\n"); |
Sigma884 | 0:a38bf4917128 | 1093 | XB.printf("ADXL375 : OK\r\n"); |
Sigma884 | 0:a38bf4917128 | 1094 | strcat(setup_string, "ADXL375 : OK\r\n"); |
Sigma884 | 0:a38bf4917128 | 1095 | } |
Sigma884 | 0:a38bf4917128 | 1096 | else{ |
Sigma884 | 0:a38bf4917128 | 1097 | pc.printf("ADXL375 : NG...\r\n"); |
Sigma884 | 0:a38bf4917128 | 1098 | XB.printf("ADXL375 : NG...\r\n"); |
Sigma884 | 0:a38bf4917128 | 1099 | strcat(setup_string, "ADXL375 : NG...\r\n"); |
Sigma884 | 0:a38bf4917128 | 1100 | } |
Sigma884 | 0:a38bf4917128 | 1101 | |
Sigma884 | 0:a38bf4917128 | 1102 | ADXL375.offset(-0.3f, -0.6f, 0.3f); |
Sigma884 | 0:a38bf4917128 | 1103 | |
Sigma884 | 0:a38bf4917128 | 1104 | //GPS受信待機 |
Sigma884 | 0:a38bf4917128 | 1105 | if(waitGPS){ |
Sigma884 | 0:a38bf4917128 | 1106 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 1107 | while(!GPS.gps_readable){ |
Sigma884 | 0:a38bf4917128 | 1108 | pc.printf("GPS Waiting... : %d\r", w_time); |
Sigma884 | 0:a38bf4917128 | 1109 | XB.printf("GPS Waiting... : %d\r", w_time); |
Sigma884 | 0:a38bf4917128 | 1110 | w_time ++; |
Sigma884 | 0:a38bf4917128 | 1111 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 1112 | } |
Sigma884 | 0:a38bf4917128 | 1113 | pc.printf("GPS Waiting... : %d -> OK!!\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 1114 | XB.printf("GPS Waiting... : %d -> OK!!\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 1115 | sprintf(sprint_buff, "GPS Waiting... : %d -> OK!!\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 1116 | strcat(setup_string, sprint_buff); |
Sigma884 | 0:a38bf4917128 | 1117 | } |
Sigma884 | 0:a38bf4917128 | 1118 | |
Sigma884 | 0:a38bf4917128 | 1119 | //RPi起動待機 |
Sigma884 | 0:a38bf4917128 | 1120 | if(flag_RPi_setting){ |
Sigma884 | 0:a38bf4917128 | 1121 | bool flag_RPi_ok = false; |
Sigma884 | 0:a38bf4917128 | 1122 | w_time = 0; |
Sigma884 | 0:a38bf4917128 | 1123 | while(!flag_RPi_ok){ |
Sigma884 | 0:a38bf4917128 | 1124 | if(RPi.readable()){ |
Sigma884 | 0:a38bf4917128 | 1125 | char c = RPi.getc(); |
Sigma884 | 0:a38bf4917128 | 1126 | if(c == '_'){ |
Sigma884 | 0:a38bf4917128 | 1127 | flag_RPi_ok = true; |
Sigma884 | 0:a38bf4917128 | 1128 | } |
Sigma884 | 0:a38bf4917128 | 1129 | } |
Sigma884 | 0:a38bf4917128 | 1130 | pc.printf("RPi setting : %d\r", w_time); |
Sigma884 | 0:a38bf4917128 | 1131 | XB.printf("RPi setting : %d\r", w_time); |
Sigma884 | 0:a38bf4917128 | 1132 | w_time ++; |
Sigma884 | 0:a38bf4917128 | 1133 | wait(1.0f); |
Sigma884 | 0:a38bf4917128 | 1134 | } |
Sigma884 | 0:a38bf4917128 | 1135 | pc.printf("RPi setting : %d -> OK!!\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 1136 | XB.printf("RPi setting : %d -> OK!!\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 1137 | sprintf(sprint_buff, "RPi setting : %d -> OK!!\r\n", w_time); |
Sigma884 | 0:a38bf4917128 | 1138 | strcat(setup_string, sprint_buff); |
Sigma884 | 0:a38bf4917128 | 1139 | } |
Sigma884 | 0:a38bf4917128 | 1140 | |
Sigma884 | 0:a38bf4917128 | 1141 | //まとめ |
Sigma884 | 0:a38bf4917128 | 1142 | pc.printf("All setting finished!! -> Start!!\r\n"); |
Sigma884 | 0:a38bf4917128 | 1143 | XB.printf("All setting finished!! -> Start!!\r\n"); |
Sigma884 | 0:a38bf4917128 | 1144 | strcat(setup_string, "All setting finished!! -> Start!!\r\n"); |
Sigma884 | 0:a38bf4917128 | 1145 | |
Sigma884 | 0:a38bf4917128 | 1146 | fp = fopen(file_name, "a"); |
Sigma884 | 0:a38bf4917128 | 1147 | fprintf(fp, setup_string); |
Sigma884 | 0:a38bf4917128 | 1148 | fclose(fp); |
Sigma884 | 0:a38bf4917128 | 1149 | setup_string[0] = NULL; |
Sigma884 | 0:a38bf4917128 | 1150 | } |