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