![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ムササビチームの電装です
Dependencies: MPU6050 MS5607 SDFileSystem mbed
Revision 0:b021d725d528, committed 2017-04-05
- Comitter:
- makakensanba
- Date:
- Wed Apr 05 10:45:35 2017 +0000
- Commit message:
- ?????????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Wed Apr 05 10:45:35 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MS5607.lib Wed Apr 05 10:45:35 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/yamaguch/code/MS5607/#5760862143d1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Wed Apr 05 10:45:35 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/mbed/code/SDFileSystem/#8db0d3b02cec
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Apr 05 10:45:35 2017 +0000 @@ -0,0 +1,653 @@ +/* +2017年3月 伊豆大島共同打上実験 +団体名:CORE +チーム名:ムササビ +該当電装:ロケット搭載用 + +使用部品 +・LPC1768(マイコン) +・MPU6050(加速度・ジャイロセンサ) +・MS5607(気圧・気温センサ) +・MicroSDスロット +・MG995(サーボモータ)×4 + +使用ライブラリ +・MPU6050.h + https://developer.mbed.org/teams/mbed/code/SDFileSystem/ +・MS5607.h + https://developer.mbed.org/users/yamaguch/code/MS5607/ +・SDFileSystem.h + https://developer.mbed.org/teams/mbed/code/SDFileSystem/ +*/ +#include "mbed.h" +#include "math.h" +#include "MS5607I2C.h" +#include "MPU6050.h" +#include "SDFileSystem.h" + +#define ACC_LAUNCH 4.0f//FIXME:本番は4g +#define TOP_DROP_AMOUNT 1.5f +#define TIME_REACH_TOP 12.5f + +#define RATE_LOG 1 +#define RATE_OPEN 10 +/*サーボ動作*/ +#define LOCK 0 +#define UNLOCK 1 +/*モード定義*/ +#define STANDBY 0 +#define TEST 1 +#define FLIGHT 2 +/*開放フェーズ定義*/ +#define SETUP 0 +#define LAUNCH 1 +#define RISE 2 +#define DROP 3 +/**/ +/*DCモータ動作*/ +#define FORW 0 +#define STOP 1 +#define BACK 2 +#define P0 1013.25f//海面気圧[hPa] +#define ACC 4096.0f//加速度オフセット値 + +/*目標地点の座標*/ +#define L_HOKUI 34.73416 //ムササビチーム目標地点 +#define L_TOUKEI 139.4227 +#define ECC2 0.00669437999019758 +#define A_RADIUS 6378137.000 //長半径(赤道半径)[m] +#define B_RADIUS 6356752.314245 //短半径(極半径)(WGS84)->http://yamadarake.jp/trdi/report000001.html +#define M_PI 3.1415926535897932384626433832795 +#define M_PI_2 1.57079632679489661923 +#define epsilon 1.0E-6 + +/*ピン指定*/ +MS5607I2C ms5607(p9, p10, false); +MPU6050 mpu(p9,p10); +BusOut myled(LED1,LED2,LED3,LED4); +SDFileSystem sd(p5, p6, p7, p8, "sd"); +Serial device(USBTX, USBRX); +DigitalIn flightpin(p19); +DigitalOut RED(p30); +Serial twe(p13,p14); +Serial gps(p28,p27); // tx, rx +PwmOut Door_UP(p21); +PwmOut Door_BOTTOM(p22); +PwmOut DC(p23); + + +/*タイマー類*/ +Timer timer; +Ticker loop_log; +Ticker loop_open; +/*ログカウンタ*/ +bool row = 0; +int8_t col = 0; +/*ログ格納用*/ +float alt_drop; +FILE *fp; +/*フェーズ変数*/ +int8_t Phase = SETUP; +int8_t Mode = STANDBY; +char flight[5] = {}; +int i_f =0; +/*判定用*/ +float alt_buff[RATE_OPEN]; +static float alt_max,alt_launch; +float t_drop,t_top,t_launch; +int8_t cnt_judge = 0; +int8_t col_open = 0; +/*入力用*/ +int8_t input_buff[2] = {}; +int8_t input_cnt = 0; +int u; + +/*GPS変数*/ +const float dt =0.06; +int i,mode,Step; +int j = 0; +char gps_data[256]; +char st,ns,ew; +float w_time,hokui,tokei; +float vel_norm,vel_head; + +float g_hokui,g_tokei; +float d_hokui,m_hokui,d_tokei,m_tokei; +float def; +unsigned char c; +float dy, dx; +float yAve,n,m,ecc,w; +float xDist, yDist, dist; +float x_tgt[3]= {166.7,-199.7,0.0}; + +/*関数*/ +void _Open(); +//void _Log(); +void _GPS(); +void _Motor(int8_t door_num, int8_t motion); +void _Input(); +float _Measure_Alt(float press, float temp); +float _Median(float data[], int num); +int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max); +int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt); +void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]); +void periodize(float &x, float max, float min); +float hdg_conv(float hdgN0deg); +void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]); +void segmentBisector(float a[], float v1[], float v2[]); +int intersection(float r[], float l[], float m[]); +float dot2(float a[], float b[]); +float norm2(float v[]); + +/*---------------------------------------------------------------------*/ +int main() { + device.baud(115200); + gps.baud(115200); + twe.baud(115200); + mpu.setAcceleroRange(2); + mpu.setGyroRange(2); + timer.start(); + Door_UP.period_ms(20); + Door_BOTTOM.period_ms(20); + DC.period_ms(20); + ms5607.printCoefficients(); + + // _Motor(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定? + // _Motor(2,UNLOCK);// + fp = fopen("/sd/log.txt", "a"); + device.attach(&_Input,Serial::RxIrq); + loop_open.attach(&_Open,1.0/RATE_OPEN); + while(1); +} +/*開放用関数 RATE_OPEN[Hz]で判定を行う*/ +void _Open(){ + myled = 1 << (Phase-1); + switch (Phase) { + case SETUP://セットアップモード(発射判定不可) + break; + case LAUNCH://点火モード(発射判定可) + device.printf("Phase:LAUNCH\r\n"); + + float acc_buffx2 = (float)mpu.getAcceleroRawX()/(ACC*0.981); + float acc_buffy2 = (float)mpu.getAcceleroRawY()/(ACC*0.981); + float acc_buffz2 = (float)mpu.getAcceleroRawZ()/(ACC*0.981); + + float x_acc=acc_buffx2*acc_buffx2; + float y_acc=acc_buffy2*acc_buffy2; + float z_acc=acc_buffz2*acc_buffz2; + + float acc_sum = (x_acc+y_acc+z_acc)*1.0; + alt_buff[col_open] = ms5607.getAltitude(); + if(acc_sum>ACC_LAUNCH||flightpin==1){ + if(cnt_judge++==3){ + Phase = RISE; + alt_launch = _Median(alt_buff, RATE_OPEN); + cnt_judge = 0; + } + device.printf("launch:%f",alt_launch); + t_launch = timer.read(); + alt_max = alt_launch; + }else{ + if(timer.read()>t_launch+1.0) cnt_judge = 0; + } + break; + case RISE://上昇中(パラシュート開放判定) + device.printf("Phase:RISE\r\n"); + float alt_rising = ms5607.getAltitude(); + if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising; + if(alt_rising<alt_max-TOP_DROP_AMOUNT){ + if(cnt_judge++==3){ + twe.printf("Phase:RISE.ALT Open.\r\n"); + gps.attach(&_GPS,Serial::RxIrq); + + _Motor(1,UNLOCK); + Phase = DROP; + cnt_judge = 0; + } + t_top = timer.read(); + }else{ + if(timer.read()>t_top+1.0) cnt_judge = 0; + } + if(timer.read()-t_launch>TIME_REACH_TOP){ + _Motor(1,UNLOCK); + gps.attach(&_GPS,Serial::RxIrq); + + Phase = DROP; + cnt_judge = 0; + } + break; + case DROP://降下中(缶サット開放判定) + + //device.printf("%d %c %f %f %f %f \n\r",u,st,vel_norm, vel_head, alt_drop, alt_max); + //device.printf("Phase:DROP. Input:%d /r/n",u); + + //_Measure alt 現在の高度 + dt->60ms + + break; + } + if(col_open++==RATE_OPEN) col_open = 0; +} +/* +void _Log(){ + if(t[row][col] = timer.read()>=30.0*60.0){ + timer.reset(); + t[row][col] = timer.read(); + } + pressure[row][col] = ms5607.getPressure()/100; + temperature[row][col] = ms5607.getTemperature(); + fprintf(fp,"%d,%f,%f\r\n",t[row][col],pressure[row][col],temperature[row][col]); + if(col++==RATE_LOG){ + col = 0; + row =! row; + fclose(fp); + fp = fopen("/sd/log.txt", "a"); + } +} +*/ + void _Motor(int8_t num, int8_t motion) { + if(num==1) { //扉 + if(motion==UNLOCK) { + Door_UP.pulsewidth(0.0015); + Door_BOTTOM.pulsewidth(0.0023); + wait(1.0); + Door_UP.pulsewidth(0); + Door_BOTTOM.pulsewidth(0); + } else if(motion==LOCK) { + Door_UP.pulsewidth(0.0023); + Door_BOTTOM.pulsewidth(0.0015); + wait(1.0); + Door_UP.pulsewidth(0); + Door_BOTTOM.pulsewidth(0); + } else { + device.printf("error%f\r\n",motion); + } + } else if(num==2) { //DC + if(motion==FORW) { + DC.pulsewidth(0.001); + wait(0.2); + } else if(motion==STOP) { + DC.pulsewidth(0.0015); + wait(0.2); + } else if(motion==BACK) { + DC.pulsewidth(0.002); + wait(0.2); + } else { + device.printf("error%f\r\n",motion); + } + } else { + device.printf("Motor error:%d\r\n",num); + } + } +void _GPS() { + static int cnt=0; + cnt++; + RED =1; + c = gps.getc(); + if( c=='$' || i == 256){ + mode = 0; + i = 0; + for(int j=0; j<256; j++){ + gps_data[j]=NULL; + } + } + if(mode==0){ + if((gps_data[i]=c) != '\r'){ + i++; + }else{ + gps_data[i]='\0'; + + if( sscanf(gps_data, "$GNRMC,%f,%c,%f,%c,%f,%c,%f,%f",&w_time,&st,&hokui,&ns,&tokei,&ew,&vel_norm,&vel_head) >= 1){ + //logitude + + d_tokei= int(tokei/100); + m_tokei= (tokei-d_tokei*100)/60; + g_tokei= d_tokei+m_tokei; + //Latitude + d_hokui=int(hokui/100); + m_hokui=(hokui-d_hokui*100)/60; + g_hokui=d_hokui+m_hokui; + + dy = (L_HOKUI-g_hokui)/180*M_PI; + dx = (L_TOUKEI-g_tokei)/180*M_PI; + yAve = (g_hokui+L_HOKUI)/2/180*M_PI; + w = sqrt(1-ECC2*sin(yAve)*sin(yAve)); + m = A_RADIUS*(1-ECC2)/(w*w*w); + n = A_RADIUS/w; + dist = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))/1); + xDist = dx*n*cos(yAve); + yDist = dy*m; + + float alt = ms5607.getAltitude(); + //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head); + device.printf("Radio \r\n"); + +/* + static int k=0; + if (k++ % 6 == 0){ + float alt = ms5607.getAltitude(); + //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head); + device.printf("Radio \r\n"); + + twe.printf("%c,%f %f %f %f %f %d \r\n",Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u); + fprintf(fp,"%c,%f %f %f %f %f %d \r\n" ,Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u); + + if(k % 30 == 0){ + k = 0; + row =! row; + fclose(fp); + fp = fopen("/sd/log.txt", "a"); + } + } +*/ static float alt_drop2=ms5607.getAltitude(); + alt_drop2=ms5607.getAltitude(); + alt_drop=alt_drop2-alt_launch; + + float x_s[] = { xDist, yDist }; + device.printf("x_s %f %f drp:%f max:%f \r\n",x_s[0],x_s[1],alt_drop,alt_max - alt_launch); + u = set_input(st == 'A', x_s, vel_norm, vel_head, alt_drop, alt_max - alt_launch); + device.printf("%d",u); + twe.printf("%d,%f %f %f %f %f %d \r\n",cnt,hokui,tokei,vel_norm,vel_head,alt_drop,alt_max - alt_launch,u); + + static int ui = 0; + const int ui_max = 30, ui_min = 10; // ui_min = 0 ? + if (ui < ui_min) u = 1; + else if (ui > ui_max) u = -1; + ui += u; + + if(u == -1) _Motor(2,FORW);//右旋回 + else if(u == 0) _Motor(2,STOP); + else if(u == 1) _Motor(2,BACK);//左旋回 + + + sprintf(gps_data, ""); + }//if + } + } +} + +void _Input(){ + input_buff[input_cnt] = device.getc(); + device.printf("\r\n"); + switch (Mode) { + case STANDBY: + if(input_cnt==0){ + if(input_buff[0]=='S'){ + device.printf("U >> UNLOCK\r\n"); + device.printf("L >> LOCK\r\n"); + }else if(input_buff[0]=='M'){ + device.printf("S >> STANDBY\r\n"); + device.printf("F >> FLIGHT\r\n"); + }else{ + device.printf("This command is not found >> %c\r\n",input_buff[0]); + device.printf(">>MAINMENU<<\r\n"); + device.printf("S >> Servo Operation\r\n"); + device.printf("M >> Mode Change\r\n"); + device.printf("-->>"); + return; + } + }else if(input_cnt==1){ + if(input_buff[0]=='S'){ + if(input_buff[1]=='U')_Motor(1,UNLOCK); + else if(input_buff[1]=='L')_Motor(1,LOCK); + else{ + device.printf("This command is not found >> %c\r\n",input_buff[1]); + device.printf("U >> UNLOCK\r\n"); + device.printf("L >> LOCK\r\n"); + device.printf("-->>"); + return; + } + }else if(input_buff[0]=='M'){ + if(input_buff[1]=='S'){ + Mode = STANDBY; + }else if(input_buff[1]=='F'){ + Mode = FLIGHT; + Phase = LAUNCH; + + device.printf("FLIGHT-Mode ON!\r\n"); + device.printf("***alert***\r\n"); + device.printf("You will be able to reset only!\r\n"); + return; + }else{ + device.printf("This command is not found >> %c\r\n",input_buff[1]); + device.printf("S >> STANDBY\r\n"); + device.printf("F >> FLIGHT\r\n"); + device.printf("-->>"); + return; + } + } + input_cnt = 0; + device.printf(">>MAINMENU<<\r\n"); + device.printf("S >> Servo Operation\r\n"); + device.printf("M >> Mode Change\r\n"); + device.printf("-->>"); + return; + } + device.printf("-->>"); + input_cnt++; + break; + case FLIGHT://reset only + break; + } +} + +/*その他雑関数*/ +float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){ + return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; +} +float _Median(float data[], int num){ + float median; + float *sort = (float *)malloc(sizeof(float)*num); + for(int i=0; i<num; i++) sort[i] = data[i]; + for(int i=0; i<num; i++){ + for(int j=0; j<num-i-1; j++){ + if(sort[j]>sort[j+1]){ + float buff = sort[j+1]; + sort[j+1] = sort[j]; + sort[j] = buff; + } + } + } + if(num%2!=0)median = sort[num/2]; + else median = (sort[num/2-1]+sort[num/2])/2.0; + free(sort); + return median; +} + +int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max) +{ + // グローバルにするかも? + const int dn = 40; // windstarのベクトルを取得するステップ間隔 + const float vz_est = 4.62; // パラフォイルのsinkrate[m/s] + const float dist_tgt = 30.0; // 目標旋回半径[m] + const float alt_unctrl = 50.0; // 無制御で降下する高度[m] + + static int gps_step = 1; + float u = 0.0; + if (gps_good) + { + enum Phase + { + WIND_EST, + CTRL + }; + static Phase phase = WIND_EST; + + // sensor data + float dt_ = dt*gps_step; + gps_step = 1; + + // state variable estimation + float hdg_s = hdg_conv(hdg_gps); + static float hdg0 = hdg_s; + float omg_calc = hdg_s - hdg0; + if (omg_calc >= M_PI) omg_calc -= 2 * M_PI; + else if (omg_calc <= -M_PI) omg_calc += 2 * M_PI; + omg_calc /= dt_; + hdg0 = hdg_s; + + // static float alt0 = alt_s; + // float vz_calc = (alt_s - alt0) / dt_; + // alt0 = alt_s; + + float vg_s[] = { vg_knot*0.514444444*cos(hdg_s), vg_knot*0.514444444*sin(hdg_s) }; + // if want to use estimated position, uncomment below + // float x_est[2] = {}; + // filter_x(dt_, 0.1, x_est, x_s, vg_s); + + // wind estimation sequence + static float vw_est[2] = {}; + switch (phase) + { + case WIND_EST: + { + const int dn2 = 2 * dn; + static int n = 0; + float vw_t[2] = {}; + static int cnt = 0; + if (n >= dn2) + { + static int j = 0; + static float vg_buf[dn2][2] = {}; + windstar(vw_t, vg_buf[n % dn2], vg_buf[(n + dn) % dn2], vg_s); + vw_est[0] = (vw_est[0] * j + vw_t[0]) / (j + 1); + vw_est[1] = (vw_est[1] * j + vw_t[1]) / (j + 1); + j++; + vg_buf[n % dn2][0] = vg_s[0]; + vg_buf[n % dn2][1] = vg_s[1]; + } + n++; + + if (alt_s < alt_max - alt_unctrl) // + if (cnt++ >= 3) phase = CTRL; + } + break; + case CTRL: + { + float t_flt_est = (alt_s - x_tgt[2]) / vz_est; + if (abs(vz_est) <= epsilon) t_flt_est = 0.0; + float windoffset[2] = { -vw_est[0] * t_flt_est, -vw_est[1] * t_flt_est }; + if (windoffset[1] > 200.0) windoffset[1] = 200.0; + float x_virt[2] = { x_s[0] + windoffset[0], x_s[1] + windoffset[1] }; + // if want to use estimated position, comment out above and uncomment below + // float x_virt[2] = { x_est[0] + windoffset[0], x_est[1] + windoffset[1] }; + float x_virt_rel[2] = { x_virt[0] - x_tgt[0], x_virt[1] - x_tgt[1] }; + u = control(x_virt_rel, vw_est, vg_s, hdg_s, omg_calc, dist_tgt); + } + break; + } + + } + else + gps_step++; + + return u; +} + +int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt) +{ + const float threshold = 0.20; + + float theta = atan2(r_rel[1], r_rel[0]); + + float dist = norm2(r_rel); + float Vr = dot2(r_rel, v) / dist; + float Vtheta = (r_rel[0] * v[1] - r_rel[1] * v[0]) / dist; + + float hdg_tgt = 0.0; + float alpha = atan(0.5*(dist - dist_tgt)); + float limitalpha = M_PI_2 - asin(dist_tgt / dist); + if (alpha > limitalpha) alpha = limitalpha; + if (Vtheta > 0) + hdg_tgt = theta + M_PI_2 + alpha; + else + hdg_tgt = theta - M_PI_2 - alpha; + periodize(hdg_tgt, M_PI, -M_PI); + + float dhdg = hdg - hdg_tgt; + periodize(dhdg, M_PI, -M_PI); + + float domg = omg - Vtheta / dist; + float Kp = 0.5, Kd = 3.0; + float u = -Kp*dhdg - Kd*domg; + + if (dist < 200.0) + { + if (Vtheta > 0) u += 0.05*Vr; + else u -= 0.05*Vr; + } + + if (u > threshold) return 1; + else if (u < -threshold) return -1; + else return 0; +} + +void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]) +{ + static float x0[] = { x_gps[0], x_gps[1], x_gps[2] }; + static int called = 0; + if (!called) + { + for (int i = 0; i < 2; i++) + x_est[i] = x0[i]; + called++; + return; + } + float xt[2]; + for (int i = 0; i < 2; i++) + { + xt[i] = x0[i] + vg_gps[i] * dt; + x_est[i] = C*x_gps[i] + (1 - C)*xt[i]; + x0[i] = x_est[i]; + } +} + +float hdg_conv(float hdgN0deg) +{ + hdgN0deg *= -M_PI / 180.0; + hdgN0deg += M_PI_2; + periodize(hdgN0deg, M_PI, -M_PI); + return hdgN0deg; +} +void periodize(float &x, float max, float min) +{ + float range = max - min; + while (x > max) x -= range; + while (x < min) x += range; +} + +void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]) +{ + float l[3][3] = {}; // line coeffs l[0]*x + l[1]*y + l[2] = 0 + segmentBisector(l[0], Vg0, Vg1); + segmentBisector(l[1], Vg1, Vg2); + segmentBisector(l[2], Vg2, Vg0); + float r[3][3] = {}; // intersection point + intersection(r[0], l[0], l[1]); + intersection(r[1], l[1], l[2]); + intersection(r[2], l[2], l[0]); + Vw[0] = (r[0][0] + r[1][0] + r[2][0]) / 3; + Vw[1] = (r[0][1] + r[1][1] + r[2][1]) / 3; +} +void segmentBisector(float a[], float v1[], float v2[]) +{ + a[0] = v2[0] - v1[0]; + a[1] = v2[1] - v1[1]; + a[2] = -0.5*(a[0] * (v1[0] + v2[0]) + a[1] * (v1[1] + v2[1])); +} +int intersection(float r[], float l[], float m[]) +{ + float det = l[0] * m[1] - l[1] * m[0]; + if (abs(det) < epsilon) return -1; + r[0] = (-m[1] * l[2] + l[1] * m[2]) / det; + r[1] = (m[0] * l[2] - l[0] * m[2]) / det; + return 0; +} + +float dot2(float a[], float b[]) +{ + return a[0] * b[0] + a[1] * b[1]; +} +float norm2(float v[]) +{ + return sqrt(dot2(v, v)); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Apr 05 10:45:35 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/093f2bd7b9eb \ No newline at end of file