hitomi shio
a
Dependencies: mbed SDFileSystem BMP180
Diff: main.cpp
- Revision:
- 0:0624addc6841
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 19 15:43:11 2021 +0000
@@ -0,0 +1,501 @@
+#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(p13); //ロードセル
+DigitalIn load_data(p14);
+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(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;
+
+ for(int i=0; i<10; i++){
+ getGram += _getGram();
+ wait(1.0);
+ }
+ float Gram_0 = 0.0;
+ Gram_0 = getGram/10.0;
+
+ //pc.printf("Gram_0: %f\r\n", Gram_0);
+ twe.printf("Gram_0: %f\r\n", Gram_0);
+ /*
+ fp = fopen("/sd/test.txt", "a");
+ fprintf(fp, "Gram_0: %f\r\n", Gram_0);
+ fclose(fp);
+ */
+ /*ちょっとあげる*/
+ motor2_pwm = 1.0;
+ motor2_1 = 1;
+ motor2_2 = 0;
+ wait(3.0);
+
+ /*サーボ*/
+ servo.pulsewidth(0.0015);
+ wait(1.0);
+ servo.pulsewidth(0.0018);
+ wait(1.0);
+ servo.pulsewidth(0.0020);
+ wait(1.0);
+ servo.pulsewidth(0.0022);
+ wait(1.0);
+ servo.pulsewidth(0.0023);
+ wait(1.0);
+ //servo.pulsewidth(0.0026);
+ //wait(1.0);
+ /*
+ servo.pulsewidth(0.0030);
+ */
+ /*ドリル動作 motor1:回転、motor2:下降上昇*/
+ motor2_pwm = 1.0;
+ motor2_1 = 0; //下降・回転
+ motor2_2 = 1;
+ 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 = 1; //上昇・回転停止
+ motor2_2 = 0;
+ motor1_1 = 0.0;
+ motor1_2 = 0.0;
+ wait(15.0);
+ motor2_1 = 0; //止まる・回転停止
+ motor2_2 = 0;
+ motor1_1 = 0.0;
+ motor1_2 = 0.0;
+ wait(2.0);
+
+ /*サーボ動作*/
+ servo.pulsewidth(0.0023);
+ wait(1.0);
+ servo.pulsewidth(0.0022);
+ wait(1.0);
+ servo.pulsewidth(0.0020);
+ wait(1.0);
+ servo.pulsewidth(0.0018);
+ 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;
+
+
+ /*質量値取得*/
+
+ for(int i=0; i<30; i++){
+ getGram = _getGram();
+ //pc.printf("Gram: %f \r\n", getGram - Gram_0);
+ twe.printf("Gram: %f \r\n", getGram - Gram_0);
+ /*
+ fp = fopen("/sd/test.txt", "a");
+ fprintf(fp, "Gram: %f\r\n", getGram - Gram_0);
+ fclose(fp);
+ */
+ wait(1.0);
+ }
+
+ 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();
+ 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;
+ }
+ 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;
+ }
+ 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);
+ }
+}
+
+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;
+}