20.09.29_main program test
Dependencies: mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt
main.cpp
- Committer:
- imadaemi
- Date:
- 2020-09-29
- Revision:
- 1:d208f4e139a9
- Parent:
- 0:f3a52599183f
- Child:
- 2:7bf845f17396
File content as of revision 1:d208f4e139a9:
#include "mbed.h" #include "math.h" #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.594801//投下前に計測 #define LON_GOAL 130.218002//投下前に計測 #define PI 3.14159265358979323846 #define R 6378137 Serial pc(USBTX,USBRX, 115200); Serial xbee(p28, p27, 115200); Serial openlog(p13, p14, 115200); Serial gps_serial(p13, p14, 115200); Serial xbee_raspi(p28, p27, 115200); I2C i2c(p9, p10); DigitalIn flightpin(p20); DigitalOut pin(p22); 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); GPS_interrupt gps(&gps_serial); 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; 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(); void SetLps22hb(); void GetLps22hb(); void SetGPS(); void GetGPS(); void CalculateAltitude(); void FlightPin(); void CutNichrome(float nich_wait); void NichromeOn(); void NichromeOff(); void CalcDirection(float lat, float lon, float lat_f, float lon_f); void CalcDistance(float lat, float lon); void MotorFront(float f_left,float f_right,float f_wait); void MotorBack(float f_left,float f_right,float f_wait); 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]; float press, temp; float altitude_ground, altitude; float lat, lon, height; bool nich_status = false; float cansat_direction; float cansat_distance; float lat_0, lon_0; int count_loop = 1; float percent; int main() { Phase1(); //Phase2(); //Phase3(); cansat_phase = 5; //Phase4(); rasp_data_received(); } 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); 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"); } void Phase2() //フライトピンの検知 { pc.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; } } cansat_phase = 3; pc.printf("FlightPin finished!\r\n"); //xbee.printf("FlightPin finished!\r\n"); } void Phase3() //ニクロム線による分離 { pc.printf("Phase_%d\r\n", cansat_phase); //xbee.printf("Phase_%d\r\n", cansat_phase); while(cansat_phase == 3) { 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 Phase4(){ //走行中 pc.printf("Phase_%d\r\n", cansat_phase); //xbee.printf("Phase_%d\r\n", cansat_phase); wait(3.0f); while(1) { GetGPS(); if(lat == 0) { pc.printf("GPS Data NG...\r\n"); //xbee.printf("GPS Data NG...\r\n"); } else { break; } } GetGPS(); lat_0 = lat; lon_0 = lon; MotorFront(1.0, 1.0, 3.0); while(cansat_phase == 4) { 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; 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; CalcDistance(lat, lon); 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 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"); break; case 0x82://ゴール発見 //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.printf("Go to the Right360\r\n"); MotorFront(1.0, 0, 1.0); MotorStop(); break; } } */ /*************************************************** センサのセットアップ ***************************************************/ void SetSensor() { SetMpu9250(); SetLps22hb(); SetGPS(); } void SetMpu9250() //MPU9250のセットアップ { mpu9250_test = mpu9250.sensorTest(); mag_mpu9250_test = mpu9250.mag_sensorTest(); if(mpu9250_test == true) { pc.printf("MPU9250 OK!\r\n"); //xbee.printf("MPU9250 OK!\r\n"); } else { pc.printf("MPU9250 NG...\r\n"); //xbee.printf("MPU9250 NG...\r\n"); } if(mag_mpu9250_test == true) { pc.printf("MPU9250 MAG OK!\r\n"); //xbee.printf("MPU9250 MAG OK!\r\n"); } else { pc.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のセットアップ { lps22hb.begin(); if(lps22hb.test() == true) { pc.printf("LPS22HB OK!\r\n"); //xbee.printf("LPS22HB OK!\r\n"); } else { pc.printf("LPS22HB NG...\r\n"); //xbee.printf("LPS22HB NG...\r\n"); } } void SetGPS() //GPSのセットアップ { // gps.changeGPSFreq(10); 入れるとエラーが出る pc.printf("GPS OK!\r\n"); //xbee.printf("GPS OK!\r\n"); } /*************************************************** データの取得 ***************************************************/ void GetData() { 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); } 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() { lps22hb.read_press(&press); lps22hb.read_temp(&temp); //pc.printf("%f\t%f\t",press, temp); } void GetGPS() { lat = gps.Latitude(); lon = gps.Longitude(); height = gps.Height(); //pc.printf("%f\t%f\t%f\t", lat, lon, height); //xbee.printf("%f\t%f\t%f\t", lat, lon, height); //openlog.printf("%f\t%f\t%f\t", lat, lon, height); } /*************************************************** 制御計算 ***************************************************/ void CalculateAltitude() { lps22hb.read_press(&press); 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() { //pc.printf("Hello FlightPin!\r\n"); while(cansat_phase == 2) { if(flightpin == 1) { break; } } //pc.printf("Goodbye FlightPin!\r\n"); } void CutNichrome(float nich_wait) { NichromeOn(); wait(nich_wait); NichromeOff(); } void NichromeOn() { pin = 1; nich_status = true; } void NichromeOff() { pin = 0; nich_status = false; } 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); float dlat = (lat - lat_f) * (PI / 180); float dlon = (lon - lon_f) * (PI / 180); float direc = atan2(dlat, dlon) * (180 / PI); cansat_direction = direc_g - direc; } 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; cansat_distance = sqrt(distance_squared); } /*************************************************** モーター制御 ***************************************************/ void MotorFront(float f_left,float f_right,float f_wait) { motor_left26 = 0;//正転 motor_left25 = f_left; motor_right24 = f_right; motor_right23 = 0; wait(f_wait); } void MotorBack(float b_left,float b_right,float b_wait) { motor_left26 = b_left;//逆転 motor_left25 = 0; motor_right24 = 0; motor_right23 = b_right; wait(b_wait); } void MotorStop() { motor_left26 = 0;//ストップ motor_left25 = 0; motor_right24 = 0; motor_right23 = 0; }