![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
20.09.29_main program test
Dependencies: mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt
Diff: main.cpp
- Revision:
- 0:f3a52599183f
- Child:
- 1:d208f4e139a9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Aug 27 10:10:38 2020 +0000 @@ -0,0 +1,388 @@ +#include "mbed.h" +#include "math.h" + +#include "PQLPS22HB.h" +#include "mpu9250_i2c.h" +#include "GPS_interrupt.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 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); + +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; + +void Phase1(); +void Phase2(); +void Phase3(); +void Phase4(); +void on_data_received(); +void GetData(); +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; +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; + +int main(){ + Phase4(); +} + +void Phase1(){ + pc.printf("Phase_%d\r\n", cansat_phase); + xbee.printf("Phase_%d\r\n", cansat_phase); + SetSensor(); + tick_data.attach(&GetData, 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(){//走行中 + 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(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; + + 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); + + }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); + + }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); + } + + 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(); + } +} +*/ +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"); + break; + case 0x83://ゴールが見えない + xbee_raspi.printf("cannot find red\r\n"); + 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("%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); +} + +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; +} \ No newline at end of file