Takuma Hayashi / Mbed 2 deprecated Stability_seed2019_v2

種子島ロケットコンテスト2019で0mゴールした，Space Eggs -Stability-のプログラム

Dependencies:   Nichrome_lib ADXL375_spi mbed Motor_lib LPS25H_spi GPS_interrupt

main.cpp@1:3d50114a513b, 2020-03-17 (annotated)

Committer:
Sigma884
Date:
Tue Mar 17 12:49:46 2020 +0000
Revision:
1:3d50114a513b
Parent:
0:a38bf4917128 
Re import Libraries for publish

Who changed what in which revision?

UserRevisionLine numberNew 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 }