20.09.29_main program test
Dependencies: mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt
Diff: main.cpp
- Revision:
- 1:d208f4e139a9
- Parent:
- 0:f3a52599183f
- Child:
- 2:7bf845f17396
diff -r f3a52599183f -r d208f4e139a9 main.cpp --- a/main.cpp Thu Aug 27 10:10:38 2020 +0000 +++ b/main.cpp Tue Sep 29 11:02:15 2020 +0000 @@ -4,14 +4,15 @@ #include "PQLPS22HB.h" #include "mpu9250_i2c.h" #include "GPS_interrupt.h" +#include "QQQCAM.h" #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.594909//投下前に計測 -#define LON_GOAL 130.217779//投下前に計測 +#define LAT_GOAL 33.594801//投下前に計測 +#define LON_GOAL 130.218002//投下前に計測 #define PI 3.14159265358979323846 #define R 6378137 @@ -33,6 +34,8 @@ mpu9250 mpu9250(i2c,AD0_HIGH); GPS_interrupt gps(&gps_serial); +QQQCAM cam(xbee_raspi); + Timer timer; Timeout nich_timeout; Timeout nich_wait_timeout; @@ -42,13 +45,17 @@ Ticker tick_gps; Ticker tick_time; Ticker tick_data; +Ticker tick_data_status; void Phase1(); void Phase2(); void Phase3(); void Phase4(); +void Phase5(); +void rasp_data_received(); void on_data_received(); void GetData(); +void GetStatus(); void SetSensor(); void SetMpu9250(); void GetMpu9250(); @@ -68,6 +75,9 @@ void MotorStop(); int cansat_phase = 1; +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> bool mpu9250_test; bool mag_mpu9250_test; float imu[6], mag[3]; @@ -79,39 +89,51 @@ float cansat_distance; float lat_0, lon_0; int count_loop = 1; +float percent; -int main(){ - Phase4(); +int main() +{ + Phase1(); + //Phase2(); + //Phase3(); + cansat_phase = 5; + //Phase4(); + rasp_data_received(); } -void Phase1(){ +void Phase1() +{ + //xbee_raspi.attach(on_data_received, Serial::RxIrq); pc.printf("Phase_%d\r\n", cansat_phase); - xbee.printf("Phase_%d\r\n", cansat_phase); + //xbee.printf("Phase_%d\r\n", cansat_phase); SetSensor(); tick_data.attach(&GetData, 1.0); + tick_data_status.attach(&GetStatus, 1.0); cansat_phase = 2; pc.printf("SetSensor finished!\r\n"); - xbee.printf("SetSensor finished!\r\n"); + //xbee.printf("SetSensor finished!\r\n"); } -void Phase2(){//フライトピンの検知 +void Phase2() //フライトピンの検知 +{ pc.printf("Phese_%d\r\n", cansat_phase); - xbee.printf("Phese_%d\r\n", cansat_phase); + //xbee.printf("Phese_%d\r\n", cansat_phase); timer.start(); - while(cansat_phase == 2){ - if(flightpin == 1){ - break; + while(cansat_phase == 2) { + if(flightpin == 1) { + break; } } cansat_phase = 3; pc.printf("FlightPin finished!\r\n"); - xbee.printf("FlightPin finished!\r\n"); + //xbee.printf("FlightPin finished!\r\n"); } -void Phase3(){//ニクロム線による分離 +void Phase3() //ニクロム線による分離 +{ pc.printf("Phase_%d\r\n", cansat_phase); - xbee.printf("Phase_%d\r\n", cansat_phase); - while(cansat_phase == 3){ + //xbee.printf("Phase_%d\r\n", cansat_phase); + while(cansat_phase == 3) { CutNichrome(2.0f); break; } @@ -119,17 +141,19 @@ //MotorStop(); cansat_phase = 4; pc.printf("CutNichrome finished!\r\n"); - xbee.printf("CutNichrome finished!\r\n"); + //xbee.printf("CutNichrome finished!\r\n"); } -void Phase4(){//走行中 +void Phase4(){ //走行中 + pc.printf("Phase_%d\r\n", cansat_phase); + //xbee.printf("Phase_%d\r\n", cansat_phase); wait(3.0f); - while(1){ + while(1) { GetGPS(); - if(lat == 0){ + if(lat == 0) { pc.printf("GPS Data NG...\r\n"); - xbee.printf("GPS Data NG...\r\n"); - }else{ + //xbee.printf("GPS Data NG...\r\n"); + } else { break; } } @@ -139,164 +163,268 @@ lon_0 = lon; MotorFront(1.0, 1.0, 3.0); - while(1){ - GetGPS(); - - xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); - - CalcDirection(lat, lon, lat_0, lon_0); - float direc_n = cansat_direction; + while(cansat_phase == 4) { + GetGPS(); - xbee.printf("%f\t\r\n", direc_n); - - if(-90 < direc_n && direc_n <= 0){//右へ - pc.printf("Go to the Right\r\n"); - xbee.printf("Go to the Right-90\r\n"); - MotorFront(1.0, 0, 0.25); - - }else if(-180 < direc_n && direc_n<= -90){//右へ - pc.printf("Go to the Right\r\n"); - xbee.printf("Go to the Right-180\r\n"); - MotorFront(1.0, 0, 0.5); - - }else if(-270 < direc_n && direc_n <= -180){//左へ - pc.printf("Go to the Left\r\n"); - xbee.printf("Go to the Left-270\r\n"); - MotorFront(0, 1.0, 0.5); + //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); - }else if(-360 <= direc_n && direc_n <= -270){//左へ - pc.printf("Go to the Left\r\n"); - xbee.printf("Go to the Left-360\r\n"); - MotorFront(0, 1.0, 0.25); - - }else if(0 < direc_n && direc_n<= 90){//左へ - pc.printf("Go to the Left\r\n"); - xbee.printf("Go to the Left90\r\n"); - MotorFront(0, 1.0, 0.25); - - }else if(90 < direc_n && direc_n <= 180){//左へ - pc.printf("Go to the Left\r\n"); - xbee.printf("Go to the Left180\r\n"); - MotorFront(0, 1.0, 0.5); - - }else if(180 < direc_n && direc_n<= 270){//右へ - pc.printf("Go to the Right\r\n"); - xbee.printf("Go to the Right270\r\n"); - MotorFront(1.0, 0, 0.5); + CalcDirection(lat, lon, lat_0, lon_0); + float direc_n = cansat_direction; + + if(lat == lat_0){ + driving_mode = 2; + //xbee.printf("Stack!!\n"); + MotorBack(1.0, 1.0, 1.0); + MotorBack(1.0, 0, 1.0); + MotorBack(0, 1.0, 1.0); + }else{ + driving_mode = 1; + //xbee.printf("%f\t\r\n", direc_n); + + if(-90 < direc_n && direc_n <= 0) { //右へ + direction_mode = -1; + pc.printf("Go to the Right\r\n"); + //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"); + //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"); + //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"); + //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"); + //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"); + //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"); + //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"); + //xbee.printf("Go to the Right360\r\n"); + MotorFront(1.0, 0, 0.25); + } + } + lat_0 = lat; + lon_0 = lon; - }else if(270 < direc_n && direc_n <=360){//右へ - pc.printf("Go to the Right\r\n"); - xbee.printf("Go to the Right360\r\n"); - MotorFront(1.0, 0, 0.25); - } + CalcDistance(lat, lon); - lat_0 = lat; - lon_0 = lon; - MotorFront(1.0, 1.0, 3.0); - MotorStop(); - wait(5.0); - - count_loop = count_loop + 1; - xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); - } - -} - -/* -void Phase5(){ - while(cansat_phase = 5){ - void on_data_received(); + if(cansat_distance <= 3) { + driving_mode = 0; + //xbee.printf("CanSat Goal!"); + MotorStop(); + cansat_phase = 5; + break; + } else { + MotorFront(1.0, 1.0, 3.0); + MotorStop(); + + count_loop = count_loop + 1; + //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0); + } } } -*/ -void on_data_received(){ + +void Phase5(){ + while(cansat_phase == 5){ + + } +} + +void rasp_data_received(){ + int Goal = 0; + + int flag = 0; + + while(flag == 0){ + if(cam.get_rate() == 0){ + //xbee.printf("Rasp Waiting\n"); + + }else{ + flag = 1; + } + } + + while(Goal == 0){ + pc.printf("rate:%.3f\r\n", cam.get_rate()); + //xbee.printf("rate:%.3f\r\n", cam.get_rate()); + + percent = cam.get_rate(); + + if(percent >= 0.1){ + color_mode = 2; + //pc.printf("Goal!!!"); + //xbee.printf("Goal!!!"); + tick_data_status.detach(); + tick_data.detach(); + + Goal = 1; + }else if(percent >=0.001){ + color_mode = 1; + direction_mode = 2; + //pc.printf("Red!!!"); + //xbee.printf("Red!!!"); + + MotorFront(1.0, 1.0, 1.0); + //MotorStop(); + }else{ + color_mode = 0; + direction_mode = -1; + + //pc.printf("No Red!!!"); + //xbee.printf("No Red!!!"); + MotorFront(1.0, 0.5, 1.0); + //MotorStop(); + } + MotorFront(0.45, 0.45, 2.0); + } + pc.printf("Bye!!!\n"); + cansat_phase = 6; + +} +/* +void on_data_received() +{ char data = xbee_raspi.getc(); xbee_raspi.printf("%c", data); switch(data) { case 0x81://ゴール判定 - xbee_raspi.printf("red rate > 0.1\r\n"); + //xbee_raspi.printf("red rate > 0.1\r\n"); break; case 0x82://ゴール発見 - xbee_raspi.printf("red rate > 0.01\r\n"); + //xbee_raspi.printf("red rate > 0.01\r\n"); + //xbee.printf("Go Straight\r\n"); + MotorFront(1.0, 1.0, 1.0); + MotorStop(); break; case 0x83://ゴールが見えない - xbee_raspi.printf("cannot find red\r\n"); + //xbee_raspi.printf("cannot find red\r\n"); + //xbee.printf("Go to the Right360\r\n"); + MotorFront(1.0, 0, 1.0); + MotorStop(); break; } } +*/ /*************************************************** センサのセットアップ ***************************************************/ -void SetSensor(){ +void SetSensor() +{ SetMpu9250(); SetLps22hb(); SetGPS(); } - -void SetMpu9250(){//MPU9250のセットアップ + +void SetMpu9250() //MPU9250のセットアップ +{ mpu9250_test = mpu9250.sensorTest(); mag_mpu9250_test = mpu9250.mag_sensorTest(); - if(mpu9250_test == true){ + if(mpu9250_test == true) { pc.printf("MPU9250 OK!\r\n"); - xbee.printf("MPU9250 OK!\r\n"); - }else{ + //xbee.printf("MPU9250 OK!\r\n"); + } else { pc.printf("MPU9250 NG...\r\n"); - xbee.printf("MPU9250 NG...\r\n"); + //xbee.printf("MPU9250 NG...\r\n"); } - if(mag_mpu9250_test == true){ + if(mag_mpu9250_test == true) { pc.printf("MPU9250 MAG OK!\r\n"); - xbee.printf("MPU9250 MAG OK!\r\n"); - }else{ + //xbee.printf("MPU9250 MAG OK!\r\n"); + } else { pc.printf("MPU9250 MAG NG...\r\n"); - xbee.printf("MPU9250 MAG NG...\r\n"); + //xbee.printf("MPU9250 MAG NG...\r\n"); } mpu9250.setAcc(ACC_RANGE); mpu9250.setGyro(GYRO_RANGE); mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012); } -void SetLps22hb(){//LPS22HBのセットアップ +void SetLps22hb() //LPS22HBのセットアップ +{ lps22hb.begin(); - if(lps22hb.test() == true){ + if(lps22hb.test() == true) { pc.printf("LPS22HB OK!\r\n"); - xbee.printf("LPS22HB OK!\r\n"); - }else{ + //xbee.printf("LPS22HB OK!\r\n"); + } else { pc.printf("LPS22HB NG...\r\n"); - xbee.printf("LPS22HB NG...\r\n"); + //xbee.printf("LPS22HB NG...\r\n"); } } -void SetGPS(){//GPSのセットアップ +void SetGPS() //GPSのセットアップ +{ // gps.changeGPSFreq(10); 入れるとエラーが出る pc.printf("GPS OK!\r\n"); - xbee.printf("GPS OK!\r\n"); + //xbee.printf("GPS OK!\r\n"); } /*************************************************** データの取得 ***************************************************/ -void GetData(){ +void GetData() +{ GetMpu9250(); GetLps22hb(); GetGPS(); - pc.printf("%f\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%.7f\t%.7f\t%f\t%f\t%f\t\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("%f\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%.7f\t%.7f\t%f\t%f\t%f\t\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, 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); } -void GetMpu9250(){ +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"); +} + +void GetMpu9250() +{ mpu9250.getAll(imu, mag); //pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); //xbee.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); //openlog.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t\r\n",imu[0], imu[1], imu[2], imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]); } -void GetLps22hb(){ +void GetLps22hb() +{ lps22hb.read_press(&press); lps22hb.read_temp(&temp); //pc.printf("%f\t%f\t",press, temp); } -void GetGPS(){ +void GetGPS() +{ lat = gps.Latitude(); lon = gps.Longitude(); height = gps.Height(); @@ -308,41 +436,47 @@ /*************************************************** 制御計算 ***************************************************/ -void CalculateAltitude(){ +void CalculateAltitude() +{ lps22hb.read_press(&press); - lps22hb.read_temp(&temp); + lps22hb.read_temp(&temp); altitude_ground = (powf(SEA_LEVEL_PRESS / CURRENT_LOCATION_PRESS, 1/5.257) - 1) * (CURRENT_LOCATION_TEMP + 273.15) / 0.0065; altitude = (powf(SEA_LEVEL_PRESS / press, 1/5.257) - 1) * (temp + 273.15) / 0.0065 - altitude_ground; pc.printf("%f\t\r\n",altitude); } -void FlightPin(){ +void FlightPin() +{ //pc.printf("Hello FlightPin!\r\n"); - while(cansat_phase == 2){ - if(flightpin == 1){ - break; + while(cansat_phase == 2) { + if(flightpin == 1) { + break; } } - //pc.printf("Goodbye FlightPin!\r\n"); + //pc.printf("Goodbye FlightPin!\r\n"); } -void CutNichrome(float nich_wait){ +void CutNichrome(float nich_wait) +{ NichromeOn(); wait(nich_wait); NichromeOff(); } -void NichromeOn(){ +void NichromeOn() +{ pin = 1; nich_status = true; } -void NichromeOff(){ +void NichromeOff() +{ pin = 0; nich_status = false; -} +} -void CalcDirection(float lat, float lon, float lat_f, float lon_f){ //ヒュベニの公式を使いたい +void CalcDirection(float lat, float lon, float lat_f, float lon_f) //ヒュベニの公式を使いたい +{ float dlat_g = (LAT_GOAL - lat) * (PI / 180); float dlon_g = (LON_GOAL - lon) * (PI / 180); float direc_g = atan2(dlat_g, dlon_g) * (180 / PI); @@ -354,7 +488,8 @@ cansat_direction = direc_g - direc; } -void CalcDistance(float lat, float lon){ +void CalcDistance(float lat, float lon) +{ float dlat_distance = R * (LAT_GOAL - lat) * (PI / 180); float dlon_distance = R * (LON_GOAL - lon) * (PI / 180) * cos(lat * (PI / 180)); float distance_squared = dlat_distance * dlat_distance + dlon_distance * dlon_distance; @@ -364,7 +499,8 @@ /*************************************************** モーター制御 ***************************************************/ -void MotorFront(float f_left,float f_right,float f_wait){ +void MotorFront(float f_left,float f_right,float f_wait) +{ motor_left26 = 0;//正転 motor_left25 = f_left; motor_right24 = f_right; @@ -372,7 +508,8 @@ wait(f_wait); } -void MotorBack(float b_left,float b_right,float b_wait){ +void MotorBack(float b_left,float b_right,float b_wait) +{ motor_left26 = b_left;//逆転 motor_left25 = 0; motor_right24 = 0; @@ -380,7 +517,8 @@ wait(b_wait); } -void MotorStop(){ +void MotorStop() +{ motor_left26 = 0;//ストップ motor_left25 = 0; motor_right24 = 0;