Haruki Sashida / Mbed 2 deprecated 201903_pf_lower_judgment

201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

main.cpp@4:4b3ae90ec778, 2018-08-09 (annotated)

Committer:
Yukina
Date:
Thu Aug 09 21:39:18 2018 +0000
Revision:
4:4b3ae90ec778
Parent:
3:24a8901befb6
Child:
5:f6e956e8a060
BBM

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:0ff20d8e9090 1 #include "mbed.h"
mikawataru 0:0ff20d8e9090 2 #include "math.h"
Yukina 4:4b3ae90ec778 3 #include "MPU6050.h"
mikawataru 0:0ff20d8e9090 4 #include "BMP180.h"
Yukina 4:4b3ae90ec778 5 #include "SDFileSystem.h"
Yukina 4:4b3ae90ec778 6
Yukina 4:4b3ae90ec778 7 /*マクロ処理*/
Yukina 4:4b3ae90ec778 8 //動作レート
Yukina 4:4b3ae90ec778 9 #define RATE_LOG 20
Yukina 4:4b3ae90ec778 10 #define RATE_OPEN 50
Yukina 4:4b3ae90ec778 11 //発射判定
Yukina 4:4b3ae90ec778 12 #define ACC_JUDGE_LAUNCH 1.5
Yukina 4:4b3ae90ec778 13 #define CNT_JUDGE_LAUNCH 5
Yukina 4:4b3ae90ec778 14 //待機時間
Yukina 4:4b3ae90ec778 15 #define TIME_GAP 3.0
Yukina 4:4b3ae90ec778 16 //頂点判定
Yukina 4:4b3ae90ec778 17 #define ALT_JUDGE_OPEN 1.5
Yukina 4:4b3ae90ec778 18 #define CNT_JUDGE_OPEN 5
Yukina 4:4b3ae90ec778 19 #define TIME_JUDGE_OPEN 30
Yukina 4:4b3ae90ec778 20 //放出判定
Yukina 4:4b3ae90ec778 21 #define TIME_JUDGE_OUT 60
Yukina 4:4b3ae90ec778 22 #define ALT_JUDGE_OUT -1.5
Yukina 4:4b3ae90ec778 23 #define CNT_JUDGE_OUT 5
Yukina 4:4b3ae90ec778 24 //共通
Yukina 4:4b3ae90ec778 25 #define TIME_JUDGE_CNT 1.5
Yukina 4:4b3ae90ec778 26 #define p0 1013.25f
Yukina 4:4b3ae90ec778 27 //ログ //2018.06.28 edit
Yukina 4:4b3ae90ec778 28 #define NUM_DATA 20
mikawataru 0:0ff20d8e9090 29
Yukina 4:4b3ae90ec778 30 MPU6050 mpu(p9,p10);
Yukina 4:4b3ae90ec778 31 BMP180 bmp(p9,p10);
Yukina 4:4b3ae90ec778 32 BusOut led(LED1,LED2,LED3,LED4);
Yukina 4:4b3ae90ec778 33 Serial pc(USBTX,USBRX);
Yukina 4:4b3ae90ec778 34 Serial twe(p28,p27);
Yukina 4:4b3ae90ec778 35 Serial gps(p13,p14);
Yukina 4:4b3ae90ec778 36 PwmOut servo1(p21);
Yukina 4:4b3ae90ec778 37 PwmOut servo2(p22);
Yukina 4:4b3ae90ec778 38 PwmOut servo3(p23);
Yukina 4:4b3ae90ec778 39 PwmOut servo4(p24);
Yukina 4:4b3ae90ec778 40 DigitalOut sw(p25);
Yukina 4:4b3ae90ec778 41 SDFileSystem sd(p5,p6,p7,p8,"sd");
Yukina 4:4b3ae90ec778 42 LocalFileSystem local("local");
Yukina 4:4b3ae90ec778 43
Yukina 4:4b3ae90ec778 44 Timer timer_open;
Yukina 4:4b3ae90ec778 45 Timer timer_log;
Yukina 4:4b3ae90ec778 46 Ticker tic_open;
Yukina 4:4b3ae90ec778 47 Ticker tic_log;
mikawataru 0:0ff20d8e9090 48
Yukina 4:4b3ae90ec778 49 void _open();
Yukina 4:4b3ae90ec778 50 void _log();
Yukina 4:4b3ae90ec778 51 float _getAlt();
Yukina 4:4b3ae90ec778 52 float _DMS2DEG(float raw_data);
Yukina 4:4b3ae90ec778 53 float _median(float data[],int num);
Yukina 4:4b3ae90ec778 54 int _input(char cha);
Yukina 4:4b3ae90ec778 55
Yukina 4:4b3ae90ec778 56 float Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること
Yukina 4:4b3ae90ec778 57 float Time_alt;
Yukina 4:4b3ae90ec778 58 float Data[2][NUM_DATA][8];
Yukina 4:4b3ae90ec778 59 int Cnt=0, Cnt_acc=0, Cnt_alt=0;
Yukina 4:4b3ae90ec778 60 int Cnt_data=0;
Yukina 4:4b3ae90ec778 61 int cnt_gps;
Yukina 4:4b3ae90ec778 62 char gps_data[256];
Yukina 4:4b3ae90ec778 63 bool Row;
Yukina 4:4b3ae90ec778 64 FILE *lfp;
Yukina 4:4b3ae90ec778 65 FILE *fp;
Yukina 4:4b3ae90ec778 66
Yukina 4:4b3ae90ec778 67 enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase;
Yukina 4:4b3ae90ec778 68 enum SERVO{LOCK=0,UNLOCK=1} Servo;
mikawataru 0:0ff20d8e9090 69
Yukina 4:4b3ae90ec778 70 int main(){
Yukina 4:4b3ae90ec778 71 /*センサ類初期化*/
mikawataru 0:0ff20d8e9090 72 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
Yukina 4:4b3ae90ec778 73 twe.baud(115200);
Yukina 4:4b3ae90ec778 74 sw=1;
Yukina 4:4b3ae90ec778 75 /*ファイル作成*/
Yukina 4:4b3ae90ec778 76 mkdir("/sd/mydir",0777);
Yukina 4:4b3ae90ec778 77 fp=fopen("/sd/mydir/data.txt","a");
Yukina 4:4b3ae90ec778 78 fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
Yukina 4:4b3ae90ec778 79 fclose(fp);
Yukina 4:4b3ae90ec778 80 lfp=fopen("/local/data.txt","a");
Yukina 4:4b3ae90ec778 81 fclose(lfp);
Yukina 4:4b3ae90ec778 82 /*サーボ調整*/
Yukina 4:4b3ae90ec778 83 servo1.period_ms(20);
Yukina 4:4b3ae90ec778 84 servo2.period_ms(20);
Yukina 4:4b3ae90ec778 85 servo3.period_ms(20);
Yukina 4:4b3ae90ec778 86 servo4.period_ms(20);
Yukina 4:4b3ae90ec778 87 /*初めの挨拶*/
Yukina 4:4b3ae90ec778 88 //pc.printf("POWER ON\r\n");
Yukina 4:4b3ae90ec778 89 twe.printf("POWER ON\r\n");
Yukina 4:4b3ae90ec778 90 /*入力待ち*/
Yukina 4:4b3ae90ec778 91 while(1){
Yukina 4:4b3ae90ec778 92 char cha = twe.getc();
Yukina 4:4b3ae90ec778 93 if(_input(cha)==-1){
Yukina 4:4b3ae90ec778 94 timer_log.start();
Yukina 4:4b3ae90ec778 95 tic_log.attach(&_log, 1.0/RATE_LOG);
Yukina 4:4b3ae90ec778 96 break;
Yukina 4:4b3ae90ec778 97 }
Yukina 4:4b3ae90ec778 98 }
Yukina 4:4b3ae90ec778 99 /*フライト準備*/
Yukina 4:4b3ae90ec778 100 Phase = SETUP;
Yukina 4:4b3ae90ec778 101 tic_open.attach(&_open, 1.0/RATE_OPEN);
Yukina 4:4b3ae90ec778 102
Yukina 4:4b3ae90ec778 103 /*GPS*/
Yukina 4:4b3ae90ec778 104 while(1){
Yukina 4:4b3ae90ec778 105 /*GPS取得*/
Yukina 4:4b3ae90ec778 106 if(Phase != RISE){
Yukina 4:4b3ae90ec778 107 if(gps.readable()){
Yukina 4:4b3ae90ec778 108 gps_data[cnt_gps] = gps.getc();
Yukina 4:4b3ae90ec778 109 if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
Yukina 4:4b3ae90ec778 110 cnt_gps = 0;
Yukina 4:4b3ae90ec778 111 memset(gps_data,'\0',256);
Yukina 4:4b3ae90ec778 112 }else if(gps_data[cnt_gps] == '\r'){
Yukina 4:4b3ae90ec778 113 float world_time, lon_east, lat_north;
Yukina 4:4b3ae90ec778 114 int rlock, sat_num;
Yukina 4:4b3ae90ec778 115 char lat,lon;
Yukina 4:4b3ae90ec778 116 if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
Yukina 4:4b3ae90ec778 117 if(rlock==1){
Yukina 4:4b3ae90ec778 118 lat_north = _DMS2DEG(lat_north);
Yukina 4:4b3ae90ec778 119 lon_east = _DMS2DEG(lon_east);
Yukina 4:4b3ae90ec778 120 twe.printf("%s\r\n",gps_data);
Yukina 4:4b3ae90ec778 121 twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
Yukina 4:4b3ae90ec778 122 }else{
Yukina 4:4b3ae90ec778 123 twe.printf("%s\r\n",gps_data);
Yukina 4:4b3ae90ec778 124 }
Yukina 4:4b3ae90ec778 125 }
Yukina 4:4b3ae90ec778 126 }else{
Yukina 4:4b3ae90ec778 127 cnt_gps++;
Yukina 4:4b3ae90ec778 128 }
Yukina 4:4b3ae90ec778 129 }
Yukina 4:4b3ae90ec778 130 }
Yukina 4:4b3ae90ec778 131
Yukina 4:4b3ae90ec778 132 if(timer_log.read()>=30.0*60.0){
Yukina 4:4b3ae90ec778 133 timer_log.reset();
Yukina 4:4b3ae90ec778 134 /*なんかずっとこればっか書き込まれた 2018/7/1
Yukina 4:4b3ae90ec778 135 fp=fopen("/sd/mydir/data.txt","w");
Yukina 4:4b3ae90ec778 136 fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
Yukina 4:4b3ae90ec778 137 fclose(fp);
Yukina 4:4b3ae90ec778 138 */
Yukina 4:4b3ae90ec778 139 }
mikawataru 0:0ff20d8e9090 140 }
mikawataru 0:0ff20d8e9090 141 }
mikawataru 0:0ff20d8e9090 142
Yukina 4:4b3ae90ec778 143 void _open(){
Yukina 4:4b3ae90ec778 144 led = Phase;
Yukina 4:4b3ae90ec778 145 float acc[3],acc_buf[10],acc_abs,time_acc,time_alt;
Yukina 4:4b3ae90ec778 146 switch(Phase){
Yukina 4:4b3ae90ec778 147 case SETUP: //地上高度取得
Yukina 4:4b3ae90ec778 148 //pc.printf("Hello,World!\r\n");
Yukina 4:4b3ae90ec778 149 twe.printf("Hello,World!\r\n");
Yukina 4:4b3ae90ec778 150 for(Cnt=0;Cnt<10;Cnt++){
Yukina 4:4b3ae90ec778 151 Alt_buf[Cnt] = _getAlt();
Yukina 4:4b3ae90ec778 152 }
Yukina 4:4b3ae90ec778 153 Alt_gnd = _median(Alt_buf,10);
Yukina 4:4b3ae90ec778 154 /*データをローカルファイルに保存*/
Yukina 4:4b3ae90ec778 155 lfp=fopen("/local/data.txt", "a");
Yukina 4:4b3ae90ec778 156 fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
Yukina 4:4b3ae90ec778 157 fclose(lfp);
Yukina 4:4b3ae90ec778 158
Yukina 4:4b3ae90ec778 159 timer_open.start();
Yukina 4:4b3ae90ec778 160 Phase = LAUNCH;
Yukina 4:4b3ae90ec778 161 //pc.printf("Phase = LAUNCH\r\n");
Yukina 4:4b3ae90ec778 162 twe.printf("Phase = LAUNCH\r\n");
Yukina 4:4b3ae90ec778 163 break;
Yukina 4:4b3ae90ec778 164
Yukina 4:4b3ae90ec778 165 case LAUNCH: //発射判定
Yukina 4:4b3ae90ec778 166 for(Cnt=0;Cnt<10;Cnt++){
Yukina 4:4b3ae90ec778 167 mpu.getAccelero(acc);
Yukina 4:4b3ae90ec778 168 acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2));
Yukina 4:4b3ae90ec778 169 }
Yukina 4:4b3ae90ec778 170 acc_abs = _median(acc_buf,10);
Yukina 4:4b3ae90ec778 171 //pc.printf("acc:%f\r\n",acc_abs);
Yukina 4:4b3ae90ec778 172 //pc.printf("cnt:%d\r\n",Cnt_acc);
Yukina 4:4b3ae90ec778 173 //加速度判定
Yukina 4:4b3ae90ec778 174 if(acc_abs>ACC_JUDGE_LAUNCH){
Yukina 4:4b3ae90ec778 175 Cnt_acc++;
Yukina 4:4b3ae90ec778 176 time_acc = timer_open.read();
Yukina 4:4b3ae90ec778 177 }
Yukina 4:4b3ae90ec778 178 if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0;
Yukina 4:4b3ae90ec778 179 if(Cnt_acc == CNT_JUDGE_LAUNCH){
Yukina 4:4b3ae90ec778 180 //pc.printf("Phase = WAIT\r\n");
Yukina 4:4b3ae90ec778 181 twe.printf("LAUNCHED\r\n");
Yukina 4:4b3ae90ec778 182 timer_open.reset();
Yukina 4:4b3ae90ec778 183 Phase = WAIT;
Yukina 4:4b3ae90ec778 184 }
Yukina 4:4b3ae90ec778 185 break;
Yukina 4:4b3ae90ec778 186
Yukina 4:4b3ae90ec778 187 case WAIT: //待機時間
Yukina 4:4b3ae90ec778 188 if(timer_open.read()>TIME_GAP) Phase = RISE;
Yukina 4:4b3ae90ec778 189 break;
Yukina 4:4b3ae90ec778 190
Yukina 4:4b3ae90ec778 191 case RISE: //頂点判定
Yukina 4:4b3ae90ec778 192 for(Cnt=0;Cnt<5;Cnt++){
Yukina 4:4b3ae90ec778 193 Alt_buf[Cnt] = _getAlt();
Yukina 4:4b3ae90ec778 194 //pc.printf("alt:%f\r\n",Alt_buf[Cnt]);
Yukina 4:4b3ae90ec778 195 }
Yukina 4:4b3ae90ec778 196 Alt_md = _median(Alt_buf,5);
Yukina 4:4b3ae90ec778 197 twe.printf("alt:%f\r\n",Alt_md);
Yukina 4:4b3ae90ec778 198 //twe.printf("cnt:%d\r\n",Cnt_alt);
Yukina 4:4b3ae90ec778 199 if(Alt_md > Alt_max){
Yukina 4:4b3ae90ec778 200 Alt_max = Alt_md;
Yukina 4:4b3ae90ec778 201 Cnt_alt = 0;
Yukina 4:4b3ae90ec778 202 }
Yukina 4:4b3ae90ec778 203 else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){
Yukina 4:4b3ae90ec778 204 Cnt_alt++;
Yukina 4:4b3ae90ec778 205 Time_alt = timer_open.read();
Yukina 4:4b3ae90ec778 206 }
Yukina 4:4b3ae90ec778 207 if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){
Yukina 4:4b3ae90ec778 208 Cnt_alt = 0;
Yukina 4:4b3ae90ec778 209 }
Yukina 4:4b3ae90ec778 210 if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){
Yukina 4:4b3ae90ec778 211 //pc.printf("Phase = DROP1\r\n");
Yukina 4:4b3ae90ec778 212 twe.printf("OPEN PARA\r\n");
Yukina 4:4b3ae90ec778 213 Cnt_alt = 0;
Yukina 4:4b3ae90ec778 214 /*ローカルファイルに最高高度を記録*/
Yukina 4:4b3ae90ec778 215 lfp = fopen("/local/data.txt", "a");
Yukina 4:4b3ae90ec778 216 fprintf(lfp,"最高高度：%f\r\n",Alt_max);
Yukina 4:4b3ae90ec778 217 fclose(lfp);
Yukina 4:4b3ae90ec778 218
Yukina 4:4b3ae90ec778 219 servo1.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
Yukina 4:4b3ae90ec778 220 servo2.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
Yukina 4:4b3ae90ec778 221 Phase = DROP1;
Yukina 4:4b3ae90ec778 222 }
Yukina 4:4b3ae90ec778 223 break;
Yukina 4:4b3ae90ec778 224
Yukina 4:4b3ae90ec778 225 case DROP1: //放出機構作動？
Yukina 4:4b3ae90ec778 226 for(Cnt=0;Cnt<5;Cnt++){
Yukina 4:4b3ae90ec778 227 Alt_buf[Cnt] = _getAlt();
Yukina 4:4b3ae90ec778 228 }
Yukina 4:4b3ae90ec778 229 Alt_md = _median(Alt_buf,5);
Yukina 4:4b3ae90ec778 230 twe.printf("Alt:%f\r\n",Alt_md);
Yukina 4:4b3ae90ec778 231 //twe.printf("Cnt:%d\r\n",Cnt_alt);
Yukina 4:4b3ae90ec778 232 if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){
Yukina 4:4b3ae90ec778 233 Cnt_alt++;
Yukina 4:4b3ae90ec778 234 }
Yukina 4:4b3ae90ec778 235 if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする？到達高度低いかも？
Yukina 4:4b3ae90ec778 236 //pc.printf("Phase = DROP2\r\n");
Yukina 4:4b3ae90ec778 237 twe.printf("GO\r\n");
Yukina 4:4b3ae90ec778 238 servo3.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
Yukina 4:4b3ae90ec778 239 servo4.pulsewidth(0.0006); //TODO:サーボホーンの角度調整
Yukina 4:4b3ae90ec778 240 Phase = DROP2;
Yukina 4:4b3ae90ec778 241 }
Yukina 4:4b3ae90ec778 242 break;
Yukina 4:4b3ae90ec778 243
Yukina 4:4b3ae90ec778 244 case DROP2: //着水検知．サーボの電源落とす．
Yukina 4:4b3ae90ec778 245 //twe.printf("cnt:%d\r\n",Cnt_alt);
Yukina 4:4b3ae90ec778 246 wait(3.0);
Yukina 4:4b3ae90ec778 247 sw = 0;
Yukina 4:4b3ae90ec778 248 //pc.printf("Goodbye Servo\r\n");
Yukina 4:4b3ae90ec778 249 twe.printf("FINISH\r\n");
Yukina 4:4b3ae90ec778 250 Phase=SEA;
Yukina 4:4b3ae90ec778 251 break;
Yukina 4:4b3ae90ec778 252 }
Yukina 4:4b3ae90ec778 253 }
Yukina 4:4b3ae90ec778 254
Yukina 4:4b3ae90ec778 255 void _log(){
Yukina 4:4b3ae90ec778 256 Data[Row][Cnt_data][0]=Phase;
Yukina 4:4b3ae90ec778 257 Data[Row][Cnt_data][1]=timer_log.read();
Yukina 4:4b3ae90ec778 258 mpu.getAccelero(&Data[Row][Cnt_data][2]);
Yukina 4:4b3ae90ec778 259 bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]);
Yukina 4:4b3ae90ec778 260 Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f;
Yukina 4:4b3ae90ec778 261 Cnt_data++;
Yukina 4:4b3ae90ec778 262 //pc.printf("Cnt_data:%d\r\n",Cnt_data);
Yukina 4:4b3ae90ec778 263 if(Cnt_data==NUM_DATA){
Yukina 4:4b3ae90ec778 264 Cnt_data = 0;
Yukina 4:4b3ae90ec778 265 Row =! Row;
Yukina 4:4b3ae90ec778 266 fp = fopen("/sd/mydir/data.txt","a");
Yukina 4:4b3ae90ec778 267 for(int i=0;i<NUM_DATA;i++){
Yukina 4:4b3ae90ec778 268 fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]);
Yukina 4:4b3ae90ec778 269 }
Yukina 4:4b3ae90ec778 270 fclose(fp);
Yukina 4:4b3ae90ec778 271 }
Yukina 4:4b3ae90ec778 272 }
Yukina 4:4b3ae90ec778 273
Yukina 4:4b3ae90ec778 274 float _getAlt(){
Yukina 4:4b3ae90ec778 275 float altitude,pressure,temperature;
Yukina 4:4b3ae90ec778 276 bmp.ReadData(&temperature,&pressure);
Yukina 4:4b3ae90ec778 277 altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
Yukina 4:4b3ae90ec778 278 return altitude;
Yukina 4:4b3ae90ec778 279 }
Yukina 4:4b3ae90ec778 280
Yukina 4:4b3ae90ec778 281 int _input(char cha){
Yukina 4:4b3ae90ec778 282 twe.printf("%c\r\n",cha);
Yukina 4:4b3ae90ec778 283 if(cha!='\0'){
Yukina 4:4b3ae90ec778 284 // pc.printf("%c\r\n",c);
Yukina 4:4b3ae90ec778 285 }
Yukina 4:4b3ae90ec778 286 if(cha=='F'){
Yukina 4:4b3ae90ec778 287 twe.printf("flight mode on\r\n");
Yukina 4:4b3ae90ec778 288 return -1;
Yukina 4:4b3ae90ec778 289 }else if(cha=='u'){ //開放機構のUNLOCK
Yukina 4:4b3ae90ec778 290 servo1.pulsewidth(0.0006);
Yukina 4:4b3ae90ec778 291 servo2.pulsewidth(0.0006);
Yukina 4:4b3ae90ec778 292 }else if(cha=='l'){ //開放機構のLOCK
Yukina 4:4b3ae90ec778 293 servo1.pulsewidth(0.0024);
Yukina 4:4b3ae90ec778 294 servo2.pulsewidth(0.0024);
Yukina 4:4b3ae90ec778 295 }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU)
Yukina 4:4b3ae90ec778 296 servo3.pulsewidth(0.0010);
Yukina 4:4b3ae90ec778 297 servo4.pulsewidth(0.0010);
Yukina 4:4b3ae90ec778 298 }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU)
Yukina 4:4b3ae90ec778 299 servo3.pulsewidth(0.0024);
Yukina 4:4b3ae90ec778 300 servo4.pulsewidth(0.0024);
Yukina 4:4b3ae90ec778 301 }
Yukina 4:4b3ae90ec778 302 return 0;
Yukina 4:4b3ae90ec778 303 }
Yukina 4:4b3ae90ec778 304
Yukina 4:4b3ae90ec778 305 float _DMS2DEG(float raw_data){
Yukina 4:4b3ae90ec778 306 int d=(int)(raw_data/100);
Yukina 4:4b3ae90ec778 307 float m=(raw_data-(float)d*100);
Yukina 4:4b3ae90ec778 308 return (float)d+m/60;
Yukina 4:4b3ae90ec778 309 }
Yukina 4:4b3ae90ec778 310
Yukina 4:4b3ae90ec778 311 float _median(float data[], int num){
Yukina 4:4b3ae90ec778 312 float *data_cpy, ans;
Yukina 4:4b3ae90ec778 313 data_cpy = new float[num];
Yukina 4:4b3ae90ec778 314 memcpy(data_cpy,data,sizeof(float)*num);
Yukina 4:4b3ae90ec778 315
Yukina 4:4b3ae90ec778 316 for(int i=0; i<num; i++){
Yukina 4:4b3ae90ec778 317 for(int j=0; j<num-i-1; j++){
Yukina 4:4b3ae90ec778 318 if(data_cpy[j]>data_cpy[j+1]){
Yukina 4:4b3ae90ec778 319 float buff = data_cpy[j+1];
Yukina 4:4b3ae90ec778 320 data_cpy[j+1] = data_cpy[j];
Yukina 4:4b3ae90ec778 321 data_cpy[j] = buff;
Yukina 4:4b3ae90ec778 322 }
Yukina 4:4b3ae90ec778 323 }
Yukina 4:4b3ae90ec778 324 }
Yukina 4:4b3ae90ec778 325
Yukina 4:4b3ae90ec778 326 if(num%2!=0) ans = data_cpy[num/2];
Yukina 4:4b3ae90ec778 327 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
Yukina 4:4b3ae90ec778 328 delete[] data_cpy;
Yukina 4:4b3ae90ec778 329 return ans;
Yukina 4:4b3ae90ec778 330 }