a
Dependencies: mbed SDFileSystem BMP180
main.cpp
- Committer:
- ShioHitomi
- Date:
- 2021-10-12
- Revision:
- 0:5ea8372bdf6e
- Child:
- 1:b74930a31769
File content as of revision 0:5ea8372bdf6e:
#include "mbed.h" #include "math.h" #include "BMP180.h" #include "SDFileSystem.h" #define p0 1013.25f//海面気圧 #define PI 3.1415 #define Alt_num 3.0 //何回の平均値にするか #define Angle_num 3 //何回の平均値にするか #define Angle_cnt 10 //何回姿勢角取得するか #define Alt_bou 30 //この高度より低かったらFlight mode入ったとみなす #define Z_0 0.962 //KXTC9-2050_rawを使ってキャリブレーションする #define Z_90 1.62 #define Z_180 2.24 enum PHASE{CHECK, STANDBY, FLIGHT, EXPANSION, MISSION, WITHDRAW} Phase; Serial pc(USBTX, USBRX, 115200); Serial twe(p28, p27, 115200); DigitalOut myled_1(LED1); DigitalOut myled_2(LED2); DigitalOut myled_3(LED3); DigitalOut myled_4(LED4); DigitalIn Flight_IN(p20); SDFileSystem sd(p5, p6, p7, p8, "sd"); BMP180 bmp(p9, p10);//(sda, scl) AnalogIn z(p19); //加速度センサ DigitalOut load_sck(p14); //ロードセル DigitalIn load_data(p15); DigitalOut fet(p18); //ニクロム用MOSFET PwmOut servo(p24); PwmOut motor1_1(p25); //motor1:回転用 PwmOut motor1_2(p26); DigitalOut motor2_1(p22); //motor2:上昇下降用 DigitalOut motor2_2(p23); PwmOut motor2_pwm(p21); Timer time_0; Timer time_flight; int _getTime(); //time_0取得 float _getAlt(); //高度取得 float _getAngle(); //角度取得 int _averageLoad(uint8_t times); //ロードセルのキャリブレーションする時に使う int _getLoad(); float _getGram(); //質量取得 void _Rand_judge(); //着地判定 void _Angle_judge(); int getTime; float getAlt, getAngle, getGram;; float Load_offset = 0; FILE *fp; int main(){ /*タイマー*/ time_0.start(); /*SD fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Start.\r\n"); fclose(fp); */ /*BMP180*/ bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); /*ロードセル load_sck = 1; wait_us(100); load_sck = 0; Load_offset = _averageLoad(10); */ /*サーボ*/ servo.period(0.020); Phase = CHECK; switch(Phase){ case CHECK: //pc.printf("Check mode.\r\n"); twe.printf("Check mode.\r\n"); twe.printf("Check mode.\r\n"); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Check mode.\r\n"); fclose(fp); */ /*センサ取得*/ for(int i=0; i<30; i++){ getTime = _getTime(); getAlt = _getAlt(); getAngle = _getAngle(); getGram = _getGram(); //pc.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram); twe.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram); wait(1.0); } /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Time: %d\r\n Alt: %f\r\n Angle: %f\r\n Gram: %f\r\n", getTime, getAlt, getAngle, getGram); fclose(fp); */ myled_1 = 1; Phase = STANDBY; //break; case STANDBY: //pc.printf("Standby mode.\r\n"); twe.printf("Standby mode.\r\n"); twe.printf("Standby mode.\r\n"); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Standby mode.\r\n"); fclose(fp); */ /*放出判定*/ int fly_cnt = 0; while(1) { int FlightPin = Flight_IN; getAlt = _getAlt(); //if((FlightPin == 0) && (getAlt < Alt_bou)){ if(FlightPin == 0){ time_flight.start(); break; }else{ fly_cnt++; getTime = _getTime(); getAlt = _getAlt(); getAngle = _getAngle(); if(fly_cnt == 100){ //100回に1回データ保存 fly_cnt = 0; /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Time: %d Alt: %f Angle: %f\r\n", getTime, getAlt, getAngle); fclose(fp); */ } } } myled_2 = 1; Phase = FLIGHT; case FLIGHT: //pc.printf("Flight mode.\r\n"); twe.printf("Flight mode.\r\n"); twe.printf("Flight mode.\r\n"); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Flight mode.\r\n"); fclose(fp); */ /*着地判定*/ _Rand_judge(); //pc.printf("Rand judge success!!\r\n"); twe.printf("Rand judge success!!\r\n"); twe.printf("Rand judge success!!\r\n"); myled_3 = 1; Phase = EXPANSION; case EXPANSION: //pc.printf("Expansion mode.\r\n"); twe.printf("Expansion mode.\r\n"); twe.printf("Expansion mode.\r\n"); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Expansion mode.\r\n"); fclose(fp); */ wait(30); /*ニクロム線動作*/ fet = 1; wait(0.3); fet = 0; wait(2.0); fet = 1; wait(0.3); fet = 0; wait(30.0); for(int i=0; i<30; i++){ getAngle = _getAngle(); //pc.printf("Angle: %f\r\n", getAngle); twe.printf("Angle: %f\r\n", getAngle); /* fp = fopen("/sd/test.txt", "a"); //毎回角度保存してるけどどうしようかな fprintf(fp, "Angle: %f\r\n", getAngle); fclose(fp); */ wait(1.0); } myled_4 = 1; Phase = MISSION; case MISSION: //pc.printf("Mission mode.\r\n"); twe.printf("Mission mode.\r\n"); twe.printf("Mission mode.\r\n"); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Mission mode.\r\n"); fclose(fp); */ /* load_sck = 1; wait_us(100); load_sck = 0; Load_offset = _averageLoad(10); for(int i=0; i<10; i++){ getGram += _getGram(); wait(1.0); } Load_offset = getGram/10.0; getGram = 0.0; //pc.printf("Load offset: %f\r\n", Load_offset); twe.printf("Load offset: %f\r\n", Load_offset); fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Load offset: %f\r\n", Load_offset); fclose(fp); */ /*サーボ*/ servo.pulsewidth(0.0015); //90度 wait(1.0); servo.pulsewidth(0.0018); //90度 wait(1.0); servo.pulsewidth(0.0020); //90度 wait(1.0); servo.pulsewidth(0.0022); wait(1.0); /* servo.pulsewidth(0.0024); wait(1.0); /* servo.pulsewidth(0.0028); wait(1.0); servo.pulsewidth(0.0030); */ /*ドリル動作 motor1:回転、motor2:下降上昇*/ motor2_pwm = 1.0; motor2_1 = 1; //下降・回転 motor2_2 = 0; motor1_1 = 1.0; motor1_2 = 0.0; wait(9.0); motor2_1 = 0; //止まる・回転 motor2_2 = 0; motor1_1 = 1.0; motor1_2 = 0.0; wait(2.0); motor1_1 = 0.0; //止まる・回転停止 motor1_2 = 0.0; wait(3.0); motor2_1 = 0; //上昇・回転停止 motor2_2 = 1; motor1_1 = 0.0; motor1_2 = 0.0; wait(13.0); motor2_1 = 0; //止まる・回転停止 motor2_2 = 0; motor1_1 = 0.0; motor1_2 = 0.0; wait(2.0); /*サーボ動作*/ servo.pulsewidth(0.0022); //0度 wait(1.0); servo.pulsewidth(0.0020); //90度 wait(1.0); servo.pulsewidth(0.0018); //90度 wait(1.0); servo.pulsewidth(0.0014); wait(1.0); motor1_1 = 1.0; motor1_2 = 0.0; wait(10.0); motor1_1 = 0.0; motor1_2 = 0.0; /*質量値取得 load_sck = 1; wait_us(100); load_sck = 0; Load_offset = _averageLoad(10); for(int i=0; i<10; i++){ getGram = _getGram(); //pc.printf("Gram: %f \r\n", getGram); twe.printf("Gram: %f \r\n", getGram); fp = fopen("/sd/test.txt", "a"); //最後の一回だけ保存してるけどどうしようかな fprintf(fp, "Gram: %f\r\n", getGram); fclose(fp); wait(1.0); } /* fp = fopen("/sd/test.txt", "a"); //最後の一回だけ保存してるけどどうしようかな fprintf(fp, "Gram: %f\r\n", getGram - Load_offset); fclose(fp); */ Phase = WITHDRAW; case WITHDRAW: //pc.printf("Withdraw mode.\r\n"); twe.printf("Withdraw mode.\r\n"); twe.printf("Withdraw mode.\r\n"); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Withdraw mode.\r\n"); fclose(fp); */ while(1){ myled_1 = !myled_1; myled_2 = !myled_2; myled_3 = !myled_3; myled_4 = !myled_4; getTime = _getTime(); //pc.printf("Time: %d\r\n", getTime); twe.printf("Time: %d\r\n", getTime); wait(1.0); } } } int _getTime(){ //Timer_0取得 int time; time = time_0.read(); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Time: %d\r\n",time); fclose(fp); */ //pc.printf("%d\r\n", time); return time; } float _getAlt(){ //高度取得 float Tem, Pre, Alt, sum = 0; for(int i=0; i<Alt_num; i++){ bmp.ReadData(&Tem, &Pre); Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f; sum += Alt; } /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Alt: %f\r\n",sum/Alt_num); fclose(fp); */ //pc.printf("%f, %f, %f\r\n",Tem, Pre, Alt); //pc.printf("Alt: %f\r\n", sum/Alt_num); return sum/Alt_num; } float _getAngle(){ float Z, rad, angle, Angle_sum = 0; for(int i=0; i<Angle_num; i++){ Z = 3.3*z.read(); if(Z <= Z_0){ angle = 0; }else if(Z > Z_0 && Z < Z_90){ rad = asin((Z - Z_0)/(Z_90 - Z_0)); angle = 180*rad/PI; }else if(Z >= Z_90 && Z < Z_180){ rad = asin((Z - Z_90)/(Z_180 - Z_90)); angle = 90+180*rad/PI; }else{ angle = 180; } /* if(Z <= Z_180){ angle = 180; }else if(Z > Z_180 && Z < Z_90){ rad = asin((Z - Z_180)/(Z_90 - Z_180)); angle = 180 - 180*rad/PI; }else if(Z >= Z_90 && Z < Z_0){ rad = asin((Z - Z_90)/(Z_0 - Z_90)); angle = 90 - 180*rad/PI; }else{ angle = 0; //全部-180した } */ Angle_sum += angle; } //pc.printf("Z:%f ,%f °\r\n", Z, Angle_sum/Angle_num); return Angle_sum/Angle_num; } void _Rand_judge(){ float Alt, Alt_old, Alt_new; float Alt_sum = 0; float Pre, Tem; int rand_cnt = 0; int flight_time; for(int i=0; i<Alt_num; i++){ bmp.ReadData(&Tem, &Pre); Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f; Alt_sum += Alt; } Alt_old = Alt_sum/Alt_num; Alt_sum = 0; while(1) { for(int j=0; j<Alt_num; j++){ bmp.ReadData(&Tem, &Pre); Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f; Alt_sum += Alt; } Alt_new = Alt_sum/Alt_num; Alt_sum = 0; flight_time = time_flight.read(); /* //毎回データ保存してるけどどうしよ fp = fopen("/sd/test.txt", "a"); fprintf(fp, "flight_time: %d , Alt_new: %f, rand_cnt: %d\r\n", flight_time, Alt_new, rand_cnt); fclose(fp); */ if(fabsf(Alt_new-Alt_old)<0.8){ //何m以内の時着地判定クリアとするか rand_cnt++; Alt_old = Alt_new; wait(0.97); //1Hzになるように調整 if(rand_cnt == 3){ //3回連続 または 60秒 break; } }else if(flight_time >= 60){ break; }else{ rand_cnt = 0; Alt_old = Alt_new; wait(0.97); } //pc.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt); twe.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt); } } /* void _Angle_judge(){ int i; float Angle; int success_cnt = 0; Angle = _getAngle(); Angle = _getAngle(); //なぜか最初の二回は正しい値が出ない for(i=0; i<Angle_cnt; i++){ Angle = _getAngle(); pc.printf("Angle: %f°, cnt: %d\r\n", Angle, success_cnt); //twe.printf("Angle: %f°, cnt: %d\r\n", Angle, success_cnt); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Angle: %f°, cnt: %d\r\n", Angle, success_cnt); fclose(fp); wait(1.0); if(Angle < 90){ success_cnt++; if(success_cnt == 3){ //3回連続成功すれば pc.printf("Angle judge success."); //twe.printf("Angle judge success."); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Angle judge success.\r\n"); fclose(fp); break; } }else{ success_cnt = 0; if((i+1) == Angle_cnt){ pc.printf("Angle judge faild."); //twe.printf("Angle judge faild."); /* fp = fopen("/sd/test.txt", "a"); fprintf(fp, "Angle judge failed.\r\n"); fclose(fp); break; } } } } */ int _averageLoad(uint8_t times){ int sum = 0; for (int i = 0; i < times; i++){ sum += _getLoad(); } return sum / times; } int _getLoad(){ int buffer; buffer = 0 ; while (load_data.read()==1); for (uint8_t i = 24; i--;){ load_sck=1; buffer = buffer << 1 ; if (load_data.read()){ buffer ++; } load_sck=0; } for (int i = 0; i < 1; i++){ load_sck=1; load_sck=0; } buffer = buffer ^ 0x800000; return buffer; } float _getGram(){ long val = (_averageLoad(2) - Load_offset); if(val < 0) val = 0; float scale = 0.0003*4.2987*128.0/100.0; //定格出力:0.0006V, 定格容量:100g float volt; float gram; volt = val*(4.2987/16777216.0); gram = volt/scale; return (float) gram; }