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