QQQ
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
--- 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");