20.09.29_main program test
Dependencies: mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt
Diff: main.cpp
- Revision:
- 2:7bf845f17396
- Parent:
- 1:d208f4e139a9
- Child:
- 3:b48cac06f913
diff -r d208f4e139a9 -r 7bf845f17396 main.cpp --- a/main.cpp Tue Sep 29 11:02:15 2020 +0000 +++ b/main.cpp Sun Nov 29 11:01:32 2020 +0000 @@ -9,10 +9,10 @@ #define ACC_RANGE _16G #define GYRO_RANGE _2000DPS #define SEA_LEVEL_PRESS 1029.3//投下前に設定 -#define CURRENT_LOCATION_PRESS 1023.8//投下前に設定 -#define CURRENT_LOCATION_TEMP 16.6//投下前に設定 -#define LAT_GOAL 33.594801//投下前に計測 -#define LON_GOAL 130.218002//投下前に計測 +#define CURRENT_LOCATION_PRESS 1020.624268//投下前に設定 +#define CURRENT_LOCATION_TEMP 10.650000//投下前に設定 +#define LAT_GOAL 33.594910//投下前に計測 +#define LON_GOAL 130.217892//投下前に計測 #define PI 3.14159265358979323846 #define R 6378137 @@ -22,13 +22,21 @@ Serial gps_serial(p13, p14, 115200); Serial xbee_raspi(p28, p27, 115200); I2C i2c(p9, p10); -DigitalIn flightpin(p20); +DigitalIn flightpin(p8); DigitalOut pin(p22); +//発注基板用 +PwmOut motor_left26(p23);//右IN2 +PwmOut motor_left25(p24);//右IN1 +PwmOut motor_right24(p25);//左IN1 +PwmOut motor_right23(p26);//左IN2 + +/*切削基板用 PwmOut motor_left26(p26);//左IN2 PwmOut motor_left25(p25);//左IN1 PwmOut motor_right24(p24);//右IN1 PwmOut motor_right23(p23);//右IN2 +*/ LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW); mpu9250 mpu9250(i2c,AD0_HIGH); @@ -37,13 +45,13 @@ QQQCAM cam(xbee_raspi); Timer timer; -Timeout nich_timeout; -Timeout nich_wait_timeout; -Timeout motor_timeout; -Ticker tick_mpu9250; -Ticker tick_lps22hb; -Ticker tick_gps; -Ticker tick_time; +//Timeout nich_timeout; +//Timeout nich_wait_timeout; +//Timeout motor_timeout; +//Ticker tick_mpu9250; +//Ticker tick_lps22hb; +//Ticker tick_gps; +//Ticker tick_time; Ticker tick_data; Ticker tick_data_status; @@ -64,6 +72,7 @@ void SetGPS(); void GetGPS(); void CalculateAltitude(); +float CalcAltitude_02(); void FlightPin(); void CutNichrome(float nich_wait); void NichromeOn(); @@ -78,27 +87,32 @@ int driving_mode = 0;//1:running, 2:stack int direction_mode = 0;//-1:right, 1:left, 2:straight int color_mode = 0;//0:>0.001, 1:>0.1, 2:0.1> +int cansat_goal = 0; +float phase5_time = 0; bool mpu9250_test; bool mag_mpu9250_test; float imu[6], mag[3]; float press, temp; -float altitude_ground, altitude; +float altitude_ground, altitude, altitude_02; float lat, lon, height; bool nich_status = false; float cansat_direction; float cansat_distance; float lat_0, lon_0; int count_loop = 1; +int count_data = 1; float percent; int main() { + flightpin.mode(PullUp); + cansat_phase = 1; Phase1(); - //Phase2(); - //Phase3(); - cansat_phase = 5; + Phase2(); + Phase3(); + //cansat_phase = 5; //Phase4(); - rasp_data_received(); + //rasp_data_received(); } void Phase1() @@ -129,6 +143,27 @@ //xbee.printf("FlightPin finished!\r\n"); } +void Phase3(){//気圧による + pc.printf("Phase_%d\r\n", cansat_phase); + //xbee.printf("Phase_%d\r\n", cansat_phase); + int count_al = 0; + while(cansat_phase == 3) { + if(CalcAltitude_02() >= 5){ + //xbee.printf("%d\r\n", count_al); + //count_al++; + }else if(CalcAltitude_02() < 5){ + CutNichrome(2.0f); + break; + } + } + //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる + //MotorStop(); + cansat_phase = 4; + pc.printf("CutNichrome finished!\r\n"); + //xbee.printf("CutNichrome finished!\r\n"); +} + +/* void Phase3() //ニクロム線による分離 { pc.printf("Phase_%d\r\n", cansat_phase); @@ -143,21 +178,24 @@ pc.printf("CutNichrome finished!\r\n"); //xbee.printf("CutNichrome finished!\r\n"); } +*/ void Phase4(){ //走行中 pc.printf("Phase_%d\r\n", cansat_phase); //xbee.printf("Phase_%d\r\n", cansat_phase); - wait(3.0f); + //wait(3.0f); + while(1) { GetGPS(); - if(lat == 0) { + if(lat < 5) { pc.printf("GPS Data NG...\r\n"); - //xbee.printf("GPS Data NG...\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); + wait(3.0f); } else { break; } } - + GetGPS(); lat_0 = lat; lon_0 = lon; @@ -173,7 +211,9 @@ if(lat == lat_0){ driving_mode = 2; - //xbee.printf("Stack!!\n"); + pc.printf("lat_0 = %f, lon_0 = %f\r\n",lat_0, lon_0); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); + pc.printf("Stack!!\n"); MotorBack(1.0, 1.0, 1.0); MotorBack(1.0, 0, 1.0); MotorBack(0, 1.0, 1.0); @@ -184,48 +224,56 @@ if(-90 < direc_n && direc_n <= 0) { //右へ direction_mode = -1; pc.printf("Go to the Right\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Right-90\r\n"); MotorFront(1.0, 0, 0.25); } else if(-180 < direc_n && direc_n<= -90) { //右へ direction_mode = -1; pc.printf("Go to the Right\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Right-180\r\n"); MotorFront(1.0, 0, 0.5); } else if(-270 < direc_n && direc_n <= -180) { //左へ direction_mode = 1; pc.printf("Go to the Left\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Left-270\r\n"); MotorFront(0, 1.0, 0.5); } else if(-360 <= direc_n && direc_n <= -270) { //左へ direction_mode = 1; pc.printf("Go to the Left\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Left-360\r\n"); MotorFront(0, 1.0, 0.25); } else if(0 < direc_n && direc_n<= 90) { //左へ direction_mode = 1; pc.printf("Go to the Left\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Left90\r\n"); MotorFront(0, 1.0, 0.25); } else if(90 < direc_n && direc_n <= 180) { //左へ direction_mode = 1; pc.printf("Go to the Left\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Left180\r\n"); MotorFront(0, 1.0, 0.5); } else if(180 < direc_n && direc_n<= 270) { //右へ direction_mode = -1; pc.printf("Go to the Right\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Right270\r\n"); MotorFront(1.0, 0, 0.5); } else if(270 < direc_n && direc_n <=360) { //右へ direction_mode = -1; pc.printf("Go to the Right\r\n"); + pc.printf("lat = %f, lon = %f\r\n",lat, lon); //xbee.printf("Go to the Right360\r\n"); MotorFront(1.0, 0, 0.25); } @@ -259,9 +307,10 @@ void rasp_data_received(){ int Goal = 0; + phase5_time = timer.read(); - int flag = 0; - + //int flag = 0; + /* while(flag == 0){ if(cam.get_rate() == 0){ //xbee.printf("Rasp Waiting\n"); @@ -270,20 +319,28 @@ flag = 1; } } + */ while(Goal == 0){ - pc.printf("rate:%.3f\r\n", cam.get_rate()); + //pc.printf("rate:%.3f\r\n", cam.get_rate()); //xbee.printf("rate:%.3f\r\n", cam.get_rate()); + if(120 <= timer.read() - phase5_time){ + cansat_phase = 6; + cansat_goal = 1; + Goal = 1; + } + percent = cam.get_rate(); if(percent >= 0.1){ color_mode = 2; //pc.printf("Goal!!!"); //xbee.printf("Goal!!!"); - tick_data_status.detach(); - tick_data.detach(); - + //tick_data_status.detach(); + //tick_data.detach(); + cansat_phase = 6; + cansat_goal = 1; Goal = 1; }else if(percent >=0.001){ color_mode = 1; @@ -292,7 +349,8 @@ //xbee.printf("Red!!!"); MotorFront(1.0, 1.0, 1.0); - //MotorStop(); + //MotorFront(0, 1.0, 0.3); + //MotorFront(0, 0, 1.0); }else{ color_mode = 0; direction_mode = -1; @@ -300,12 +358,14 @@ //pc.printf("No Red!!!"); //xbee.printf("No Red!!!"); MotorFront(1.0, 0.5, 1.0); - //MotorStop(); + //MotorFront(0, 0, 1.0); } - MotorFront(0.45, 0.45, 2.0); + MotorFront(0.1, 0.1, 2.0); + //MotorFront(0, 0, 2.0); } + MotorStop(); pc.printf("Bye!!!\n"); - cansat_phase = 6; + } /* @@ -392,20 +452,29 @@ GetMpu9250(); GetLps22hb(); GetGPS(); - pc.printf("%7.2f,%d,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%7.2f,%4.1f,%f,%11.7f,%12.7f,%6.2f,%8.2f,%6.2f\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude, lat, lon, height, cansat_direction, cansat_distance); - openlog.printf("%7.2f,%d,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%7.2f,%4.1f,%f,%11.7f,%12.7f,%6.2f,%8.2f,%6.2f\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude, lat, lon, height, cansat_direction, cansat_distance); + //pc.printf("%7.2f,%d,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%7.2f,%4.1f,%f,%11.7f,%12.7f,%6.2f,%8.2f,%6.2f\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude_02, lat, lon, height, cansat_direction, cansat_distance); + openlog.printf("%7.2f,%d,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%9.3f,%7.2f,%4.1f,%f,%11.7f,%12.7f,%6.2f,%8.2f,%6.2f\r\n", timer.read(), cansat_phase, imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2], press, temp, altitude_02, lat, lon, height, cansat_direction, cansat_distance); } void GetStatus() { - xbee.printf("**************************************\n"); - xbee.printf("Phase : %d\n", cansat_phase); - xbee.printf("Driving Mode : %d\n", driving_mode); - xbee.printf("Direction : %d\n", direction_mode); - xbee.printf("Distance : %.2f\n", cansat_distance); - xbee.printf("Color Percent: %f\n", percent); - xbee.printf("Color : %d\n", color_mode); - xbee.printf("**************************************\n"); + xbee.printf("**************************************\r\n"); + xbee.printf("Phase : %d\r\n", cansat_phase); + xbee.printf("Altitude_02 : %.3f\r\n", CalcAltitude_02()); + xbee.printf("Driving Mode : %d\r\n", driving_mode); + xbee.printf("Direction : %d\r\n", direction_mode); + xbee.printf("Distance : %.2f\r\n", cansat_distance); + xbee.printf("lat : %.2f\r\n", lat); + xbee.printf("lat_0 : %.2f\r\n", lat_0); + xbee.printf("lon : %.2f\r\n", lon); + xbee.printf("lon_0 : %.2f\r\n", lon_0); + xbee.printf("Color Percent: %f\r\n", percent); + xbee.printf("Color : %d\r\n", color_mode); + xbee.printf("Phase5 Time : %f\r\n", timer.read() - phase5_time); + xbee.printf("Goal : %d\r\n", cansat_goal); + xbee.printf("Counter : %d\r\n", count_data); + xbee.printf("**************************************\r\n"); + count_data++; } void GetMpu9250() @@ -445,6 +514,14 @@ pc.printf("%f\t\r\n",altitude); } +float CalcAltitude_02(){ + lps22hb.read_press(&press); + lps22hb.read_temp(&temp); + altitude_02 = (CURRENT_LOCATION_TEMP + 273.15)/0.0065*(1 - powf(press/CURRENT_LOCATION_PRESS, 287 * 0.0065/9.80665)); + //pc.printf("%f\t%f\t\r\n",press, altitude_02); + return altitude_02; +} + void FlightPin() { //pc.printf("Hello FlightPin!\r\n");