QQQ / Mbed 2 deprecated qqq_izu_main_test

20.09.29_main program test

Dependencies:   mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt

main.cpp@2:7bf845f17396, 2020-11-29 (annotated)

Committer:
imadaemi
Date:
Sun Nov 29 11:01:32 2020 +0000
Revision:
2:7bf845f17396
Parent:
1:d208f4e139a9
Child:
3:b48cac06f913
20.11.29 main program test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
imadaemi 0:f3a52599183f 1 #include "mbed.h"
imadaemi 0:f3a52599183f 2 #include "math.h"
imadaemi 0:f3a52599183f 3
imadaemi 0:f3a52599183f 4 #include "PQLPS22HB.h"
imadaemi 0:f3a52599183f 5 #include "mpu9250_i2c.h"
imadaemi 0:f3a52599183f 6 #include "GPS_interrupt.h"
imadaemi 1:d208f4e139a9 7 #include "QQQCAM.h"
imadaemi 0:f3a52599183f 8
imadaemi 0:f3a52599183f 9 #define ACC_RANGE _16G
imadaemi 0:f3a52599183f 10 #define GYRO_RANGE _2000DPS
imadaemi 0:f3a52599183f 11 #define SEA_LEVEL_PRESS 1029.3//投下前に設定
imadaemi 2:7bf845f17396 12 #define CURRENT_LOCATION_PRESS 1020.624268//投下前に設定
imadaemi 2:7bf845f17396 13 #define CURRENT_LOCATION_TEMP 10.650000//投下前に設定
imadaemi 2:7bf845f17396 14 #define LAT_GOAL 33.594910//投下前に計測
imadaemi 2:7bf845f17396 15 #define LON_GOAL 130.217892//投下前に計測
imadaemi 0:f3a52599183f 16 #define PI 3.14159265358979323846
imadaemi 0:f3a52599183f 17 #define R 6378137
imadaemi 0:f3a52599183f 18
imadaemi 0:f3a52599183f 19 Serial pc(USBTX,USBRX, 115200);
imadaemi 0:f3a52599183f 20 Serial xbee(p28, p27, 115200);
imadaemi 0:f3a52599183f 21 Serial openlog(p13, p14, 115200);
imadaemi 0:f3a52599183f 22 Serial gps_serial(p13, p14, 115200);
imadaemi 0:f3a52599183f 23 Serial xbee_raspi(p28, p27, 115200);
imadaemi 0:f3a52599183f 24 I2C i2c(p9, p10);
imadaemi 2:7bf845f17396 25 DigitalIn flightpin(p8);
imadaemi 0:f3a52599183f 26 DigitalOut pin(p22);
imadaemi 0:f3a52599183f 27
imadaemi 2:7bf845f17396 28 //発注基板用
imadaemi 2:7bf845f17396 29 PwmOut motor_left26(p23);//右IN2
imadaemi 2:7bf845f17396 30 PwmOut motor_left25(p24);//右IN1
imadaemi 2:7bf845f17396 31 PwmOut motor_right24(p25);//左IN1
imadaemi 2:7bf845f17396 32 PwmOut motor_right23(p26);//左IN2
imadaemi 2:7bf845f17396 33
imadaemi 2:7bf845f17396 34 /*切削基板用
imadaemi 0:f3a52599183f 35 PwmOut motor_left26(p26);//左IN2
imadaemi 0:f3a52599183f 36 PwmOut motor_left25(p25);//左IN1
imadaemi 0:f3a52599183f 37 PwmOut motor_right24(p24);//右IN1
imadaemi 0:f3a52599183f 38 PwmOut motor_right23(p23);//右IN2
imadaemi 2:7bf845f17396 39 */
imadaemi 0:f3a52599183f 40
imadaemi 0:f3a52599183f 41 LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
imadaemi 0:f3a52599183f 42 mpu9250 mpu9250(i2c,AD0_HIGH);
imadaemi 0:f3a52599183f 43 GPS_interrupt gps(&gps_serial);
imadaemi 0:f3a52599183f 44
imadaemi 1:d208f4e139a9 45 QQQCAM cam(xbee_raspi);
imadaemi 1:d208f4e139a9 46
imadaemi 0:f3a52599183f 47 Timer timer;
imadaemi 2:7bf845f17396 48 //Timeout nich_timeout;
imadaemi 2:7bf845f17396 49 //Timeout nich_wait_timeout;
imadaemi 2:7bf845f17396 50 //Timeout motor_timeout;
imadaemi 2:7bf845f17396 51 //Ticker tick_mpu9250;
imadaemi 2:7bf845f17396 52 //Ticker tick_lps22hb;
imadaemi 2:7bf845f17396 53 //Ticker tick_gps;
imadaemi 2:7bf845f17396 54 //Ticker tick_time;
imadaemi 0:f3a52599183f 55 Ticker tick_data;
imadaemi 1:d208f4e139a9 56 Ticker tick_data_status;
imadaemi 0:f3a52599183f 57
imadaemi 0:f3a52599183f 58 void Phase1();
imadaemi 0:f3a52599183f 59 void Phase2();
imadaemi 0:f3a52599183f 60 void Phase3();
imadaemi 0:f3a52599183f 61 void Phase4();
imadaemi 1:d208f4e139a9 62 void Phase5();
imadaemi 1:d208f4e139a9 63 void rasp_data_received();
imadaemi 0:f3a52599183f 64 void on_data_received();
imadaemi 0:f3a52599183f 65 void GetData();
imadaemi 1:d208f4e139a9 66 void GetStatus();
imadaemi 0:f3a52599183f 67 void SetSensor();
imadaemi 0:f3a52599183f 68 void SetMpu9250();
imadaemi 0:f3a52599183f 69 void GetMpu9250();
imadaemi 0:f3a52599183f 70 void SetLps22hb();
imadaemi 0:f3a52599183f 71 void GetLps22hb();
imadaemi 0:f3a52599183f 72 void SetGPS();
imadaemi 0:f3a52599183f 73 void GetGPS();
imadaemi 0:f3a52599183f 74 void CalculateAltitude();
imadaemi 2:7bf845f17396 75 float CalcAltitude_02();
imadaemi 0:f3a52599183f 76 void FlightPin();
imadaemi 0:f3a52599183f 77 void CutNichrome(float nich_wait);
imadaemi 0:f3a52599183f 78 void NichromeOn();
imadaemi 0:f3a52599183f 79 void NichromeOff();
imadaemi 0:f3a52599183f 80 void CalcDirection(float lat, float lon, float lat_f, float lon_f);
imadaemi 0:f3a52599183f 81 void CalcDistance(float lat, float lon);
imadaemi 0:f3a52599183f 82 void MotorFront(float f_left,float f_right,float f_wait);
imadaemi 0:f3a52599183f 83 void MotorBack(float f_left,float f_right,float f_wait);
imadaemi 0:f3a52599183f 84 void MotorStop();
imadaemi 0:f3a52599183f 85
imadaemi 0:f3a52599183f 86 int cansat_phase = 1;
imadaemi 1:d208f4e139a9 87 int driving_mode = 0;//1:running, 2:stack
imadaemi 1:d208f4e139a9 88 int direction_mode = 0;//-1:right, 1:left, 2:straight
imadaemi 1:d208f4e139a9 89 int color_mode = 0;//0:>0.001, 1:>0.1, 2:0.1>
imadaemi 2:7bf845f17396 90 int cansat_goal = 0;
imadaemi 2:7bf845f17396 91 float phase5_time = 0;
imadaemi 0:f3a52599183f 92 bool mpu9250_test;
imadaemi 0:f3a52599183f 93 bool mag_mpu9250_test;
imadaemi 0:f3a52599183f 94 float imu[6], mag[3];
imadaemi 0:f3a52599183f 95 float press, temp;
imadaemi 2:7bf845f17396 96 float altitude_ground, altitude, altitude_02;
imadaemi 0:f3a52599183f 97 float lat, lon, height;
imadaemi 0:f3a52599183f 98 bool nich_status = false;
imadaemi 0:f3a52599183f 99 float cansat_direction;
imadaemi 0:f3a52599183f 100 float cansat_distance;
imadaemi 0:f3a52599183f 101 float lat_0, lon_0;
imadaemi 0:f3a52599183f 102 int count_loop = 1;
imadaemi 2:7bf845f17396 103 int count_data = 1;
imadaemi 1:d208f4e139a9 104 float percent;
imadaemi 0:f3a52599183f 105
imadaemi 1:d208f4e139a9 106 int main()
imadaemi 1:d208f4e139a9 107 {
imadaemi 2:7bf845f17396 108 flightpin.mode(PullUp);
imadaemi 2:7bf845f17396 109 cansat_phase = 1;
imadaemi 1:d208f4e139a9 110 Phase1();
imadaemi 2:7bf845f17396 111 Phase2();
imadaemi 2:7bf845f17396 112 Phase3();
imadaemi 2:7bf845f17396 113 //cansat_phase = 5;
imadaemi 1:d208f4e139a9 114 //Phase4();
imadaemi 2:7bf845f17396 115 //rasp_data_received();
imadaemi 0:f3a52599183f 116 }
imadaemi 0:f3a52599183f 117
imadaemi 1:d208f4e139a9 118 void Phase1()
imadaemi 1:d208f4e139a9 119 {
imadaemi 1:d208f4e139a9 120 //xbee_raspi.attach(on_data_received, Serial::RxIrq);
imadaemi 0:f3a52599183f 121 pc.printf("Phase_%d\r\n", cansat_phase);
imadaemi 1:d208f4e139a9 122 //xbee.printf("Phase_%d\r\n", cansat_phase);
imadaemi 0:f3a52599183f 123 SetSensor();
imadaemi 0:f3a52599183f 124 tick_data.attach(&GetData, 1.0);
imadaemi 1:d208f4e139a9 125 tick_data_status.attach(&GetStatus, 1.0);
imadaemi 0:f3a52599183f 126 cansat_phase = 2;
imadaemi 0:f3a52599183f 127 pc.printf("SetSensor finished!\r\n");
imadaemi 1:d208f4e139a9 128 //xbee.printf("SetSensor finished!\r\n");
imadaemi 0:f3a52599183f 129 }
imadaemi 0:f3a52599183f 130
imadaemi 1:d208f4e139a9 131 void Phase2() //フライトピンの検知
imadaemi 1:d208f4e139a9 132 {
imadaemi 0:f3a52599183f 133 pc.printf("Phese_%d\r\n", cansat_phase);
imadaemi 1:d208f4e139a9 134 //xbee.printf("Phese_%d\r\n", cansat_phase);
imadaemi 0:f3a52599183f 135 timer.start();
imadaemi 1:d208f4e139a9 136 while(cansat_phase == 2) {
imadaemi 1:d208f4e139a9 137 if(flightpin == 1) {
imadaemi 1:d208f4e139a9 138 break;
imadaemi 0:f3a52599183f 139 }
imadaemi 0:f3a52599183f 140 }
imadaemi 0:f3a52599183f 141 cansat_phase = 3;
imadaemi 0:f3a52599183f 142 pc.printf("FlightPin finished!\r\n");
imadaemi 1:d208f4e139a9 143 //xbee.printf("FlightPin finished!\r\n");
imadaemi 0:f3a52599183f 144 }
imadaemi 0:f3a52599183f 145
imadaemi 2:7bf845f17396 146 void Phase3(){//気圧による
imadaemi 2:7bf845f17396 147 pc.printf("Phase_%d\r\n", cansat_phase);
imadaemi 2:7bf845f17396 148 //xbee.printf("Phase_%d\r\n", cansat_phase);
imadaemi 2:7bf845f17396 149 int count_al = 0;
imadaemi 2:7bf845f17396 150 while(cansat_phase == 3) {
imadaemi 2:7bf845f17396 151 if(CalcAltitude_02() >= 5){
imadaemi 2:7bf845f17396 152 //xbee.printf("%d\r\n", count_al);
imadaemi 2:7bf845f17396 153 //count_al++;
imadaemi 2:7bf845f17396 154 }else if(CalcAltitude_02() < 5){
imadaemi 2:7bf845f17396 155 CutNichrome(2.0f);
imadaemi 2:7bf845f17396 156 break;
imadaemi 2:7bf845f17396 157 }
imadaemi 2:7bf845f17396 158 }
imadaemi 2:7bf845f17396 159 //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる
imadaemi 2:7bf845f17396 160 //MotorStop();
imadaemi 2:7bf845f17396 161 cansat_phase = 4;
imadaemi 2:7bf845f17396 162 pc.printf("CutNichrome finished!\r\n");
imadaemi 2:7bf845f17396 163 //xbee.printf("CutNichrome finished!\r\n");
imadaemi 2:7bf845f17396 164 }
imadaemi 2:7bf845f17396 165
imadaemi 2:7bf845f17396 166 /*
imadaemi 1:d208f4e139a9 167 void Phase3() //ニクロム線による分離
imadaemi 1:d208f4e139a9 168 {
imadaemi 0:f3a52599183f 169 pc.printf("Phase_%d\r\n", cansat_phase);
imadaemi 1:d208f4e139a9 170 //xbee.printf("Phase_%d\r\n", cansat_phase);
imadaemi 1:d208f4e139a9 171 while(cansat_phase == 3) {
imadaemi 0:f3a52599183f 172 CutNichrome(2.0f);
imadaemi 0:f3a52599183f 173 break;
imadaemi 0:f3a52599183f 174 }
imadaemi 0:f3a52599183f 175 //MotorFront(0.95, 1.0, 5.0);//パラシュートから離れる
imadaemi 0:f3a52599183f 176 //MotorStop();
imadaemi 0:f3a52599183f 177 cansat_phase = 4;
imadaemi 0:f3a52599183f 178 pc.printf("CutNichrome finished!\r\n");
imadaemi 1:d208f4e139a9 179 //xbee.printf("CutNichrome finished!\r\n");
imadaemi 0:f3a52599183f 180 }
imadaemi 2:7bf845f17396 181 */
imadaemi 0:f3a52599183f 182
imadaemi 1:d208f4e139a9 183 void Phase4(){ //走行中
imadaemi 1:d208f4e139a9 184 pc.printf("Phase_%d\r\n", cansat_phase);
imadaemi 1:d208f4e139a9 185 //xbee.printf("Phase_%d\r\n", cansat_phase);
imadaemi 2:7bf845f17396 186 //wait(3.0f);
imadaemi 2:7bf845f17396 187
imadaemi 1:d208f4e139a9 188 while(1) {
imadaemi 0:f3a52599183f 189 GetGPS();
imadaemi 2:7bf845f17396 190 if(lat < 5) {
imadaemi 0:f3a52599183f 191 pc.printf("GPS Data NG...\r\n");
imadaemi 2:7bf845f17396 192 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 2:7bf845f17396 193 wait(3.0f);
imadaemi 1:d208f4e139a9 194 } else {
imadaemi 0:f3a52599183f 195 break;
imadaemi 0:f3a52599183f 196 }
imadaemi 0:f3a52599183f 197 }
imadaemi 2:7bf845f17396 198
imadaemi 0:f3a52599183f 199 GetGPS();
imadaemi 0:f3a52599183f 200 lat_0 = lat;
imadaemi 0:f3a52599183f 201 lon_0 = lon;
imadaemi 0:f3a52599183f 202
imadaemi 0:f3a52599183f 203 MotorFront(1.0, 1.0, 3.0);
imadaemi 1:d208f4e139a9 204 while(cansat_phase == 4) {
imadaemi 1:d208f4e139a9 205 GetGPS();
imadaemi 0:f3a52599183f 206
imadaemi 1:d208f4e139a9 207 //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0);
imadaemi 0:f3a52599183f 208
imadaemi 1:d208f4e139a9 209 CalcDirection(lat, lon, lat_0, lon_0);
imadaemi 1:d208f4e139a9 210 float direc_n = cansat_direction;
imadaemi 1:d208f4e139a9 211
imadaemi 1:d208f4e139a9 212 if(lat == lat_0){
imadaemi 1:d208f4e139a9 213 driving_mode = 2;
imadaemi 2:7bf845f17396 214 pc.printf("lat_0 = %f, lon_0 = %f\r\n",lat_0, lon_0);
imadaemi 2:7bf845f17396 215 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 2:7bf845f17396 216 pc.printf("Stack!!\n");
imadaemi 1:d208f4e139a9 217 MotorBack(1.0, 1.0, 1.0);
imadaemi 1:d208f4e139a9 218 MotorBack(1.0, 0, 1.0);
imadaemi 1:d208f4e139a9 219 MotorBack(0, 1.0, 1.0);
imadaemi 1:d208f4e139a9 220 }else{
imadaemi 1:d208f4e139a9 221 driving_mode = 1;
imadaemi 1:d208f4e139a9 222 //xbee.printf("%f\t\r\n", direc_n);
imadaemi 1:d208f4e139a9 223
imadaemi 1:d208f4e139a9 224 if(-90 < direc_n && direc_n <= 0) { //右へ
imadaemi 1:d208f4e139a9 225 direction_mode = -1;
imadaemi 1:d208f4e139a9 226 pc.printf("Go to the Right\r\n");
imadaemi 2:7bf845f17396 227 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 228 //xbee.printf("Go to the Right-90\r\n");
imadaemi 1:d208f4e139a9 229 MotorFront(1.0, 0, 0.25);
imadaemi 1:d208f4e139a9 230
imadaemi 1:d208f4e139a9 231 } else if(-180 < direc_n && direc_n<= -90) { //右へ
imadaemi 1:d208f4e139a9 232 direction_mode = -1;
imadaemi 1:d208f4e139a9 233 pc.printf("Go to the Right\r\n");
imadaemi 2:7bf845f17396 234 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 235 //xbee.printf("Go to the Right-180\r\n");
imadaemi 1:d208f4e139a9 236 MotorFront(1.0, 0, 0.5);
imadaemi 1:d208f4e139a9 237
imadaemi 1:d208f4e139a9 238 } else if(-270 < direc_n && direc_n <= -180) { //左へ
imadaemi 1:d208f4e139a9 239 direction_mode = 1;
imadaemi 1:d208f4e139a9 240 pc.printf("Go to the Left\r\n");
imadaemi 2:7bf845f17396 241 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 242 //xbee.printf("Go to the Left-270\r\n");
imadaemi 1:d208f4e139a9 243 MotorFront(0, 1.0, 0.5);
imadaemi 1:d208f4e139a9 244
imadaemi 1:d208f4e139a9 245 } else if(-360 <= direc_n && direc_n <= -270) { //左へ
imadaemi 1:d208f4e139a9 246 direction_mode = 1;
imadaemi 1:d208f4e139a9 247 pc.printf("Go to the Left\r\n");
imadaemi 2:7bf845f17396 248 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 249 //xbee.printf("Go to the Left-360\r\n");
imadaemi 1:d208f4e139a9 250 MotorFront(0, 1.0, 0.25);
imadaemi 1:d208f4e139a9 251
imadaemi 1:d208f4e139a9 252 } else if(0 < direc_n && direc_n<= 90) { //左へ
imadaemi 1:d208f4e139a9 253 direction_mode = 1;
imadaemi 1:d208f4e139a9 254 pc.printf("Go to the Left\r\n");
imadaemi 2:7bf845f17396 255 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 256 //xbee.printf("Go to the Left90\r\n");
imadaemi 1:d208f4e139a9 257 MotorFront(0, 1.0, 0.25);
imadaemi 1:d208f4e139a9 258
imadaemi 1:d208f4e139a9 259 } else if(90 < direc_n && direc_n <= 180) { //左へ
imadaemi 1:d208f4e139a9 260 direction_mode = 1;
imadaemi 1:d208f4e139a9 261 pc.printf("Go to the Left\r\n");
imadaemi 2:7bf845f17396 262 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 263 //xbee.printf("Go to the Left180\r\n");
imadaemi 1:d208f4e139a9 264 MotorFront(0, 1.0, 0.5);
imadaemi 1:d208f4e139a9 265
imadaemi 1:d208f4e139a9 266 } else if(180 < direc_n && direc_n<= 270) { //右へ
imadaemi 1:d208f4e139a9 267 direction_mode = -1;
imadaemi 1:d208f4e139a9 268 pc.printf("Go to the Right\r\n");
imadaemi 2:7bf845f17396 269 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 270 //xbee.printf("Go to the Right270\r\n");
imadaemi 1:d208f4e139a9 271 MotorFront(1.0, 0, 0.5);
imadaemi 1:d208f4e139a9 272
imadaemi 1:d208f4e139a9 273 } else if(270 < direc_n && direc_n <=360) { //右へ
imadaemi 1:d208f4e139a9 274 direction_mode = -1;
imadaemi 1:d208f4e139a9 275 pc.printf("Go to the Right\r\n");
imadaemi 2:7bf845f17396 276 pc.printf("lat = %f, lon = %f\r\n",lat, lon);
imadaemi 1:d208f4e139a9 277 //xbee.printf("Go to the Right360\r\n");
imadaemi 1:d208f4e139a9 278 MotorFront(1.0, 0, 0.25);
imadaemi 1:d208f4e139a9 279 }
imadaemi 1:d208f4e139a9 280 }
imadaemi 1:d208f4e139a9 281 lat_0 = lat;
imadaemi 1:d208f4e139a9 282 lon_0 = lon;
imadaemi 0:f3a52599183f 283
imadaemi 1:d208f4e139a9 284 CalcDistance(lat, lon);
imadaemi 0:f3a52599183f 285
imadaemi 1:d208f4e139a9 286 if(cansat_distance <= 3) {
imadaemi 1:d208f4e139a9 287 driving_mode = 0;
imadaemi 1:d208f4e139a9 288 //xbee.printf("CanSat Goal!");
imadaemi 1:d208f4e139a9 289 MotorStop();
imadaemi 1:d208f4e139a9 290 cansat_phase = 5;
imadaemi 1:d208f4e139a9 291 break;
imadaemi 1:d208f4e139a9 292 } else {
imadaemi 1:d208f4e139a9 293 MotorFront(1.0, 1.0, 3.0);
imadaemi 1:d208f4e139a9 294 MotorStop();
imadaemi 1:d208f4e139a9 295
imadaemi 1:d208f4e139a9 296 count_loop = count_loop + 1;
imadaemi 1:d208f4e139a9 297 //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0);
imadaemi 1:d208f4e139a9 298 }
imadaemi 0:f3a52599183f 299 }
imadaemi 0:f3a52599183f 300 }
imadaemi 1:d208f4e139a9 301
imadaemi 1:d208f4e139a9 302 void Phase5(){
imadaemi 1:d208f4e139a9 303 while(cansat_phase == 5){
imadaemi 1:d208f4e139a9 304
imadaemi 1:d208f4e139a9 305 }
imadaemi 1:d208f4e139a9 306 }
imadaemi 1:d208f4e139a9 307
imadaemi 1:d208f4e139a9 308 void rasp_data_received(){
imadaemi 1:d208f4e139a9 309 int Goal = 0;
imadaemi 2:7bf845f17396 310 phase5_time = timer.read();
imadaemi 1:d208f4e139a9 311
imadaemi 2:7bf845f17396 312 //int flag = 0;
imadaemi 2:7bf845f17396 313 /*
imadaemi 1:d208f4e139a9 314 while(flag == 0){
imadaemi 1:d208f4e139a9 315 if(cam.get_rate() == 0){
imadaemi 1:d208f4e139a9 316 //xbee.printf("Rasp Waiting\n");
imadaemi 1:d208f4e139a9 317
imadaemi 1:d208f4e139a9 318 }else{
imadaemi 1:d208f4e139a9 319 flag = 1;
imadaemi 1:d208f4e139a9 320 }
imadaemi 1:d208f4e139a9 321 }
imadaemi 2:7bf845f17396 322 */
imadaemi 1:d208f4e139a9 323
imadaemi 1:d208f4e139a9 324 while(Goal == 0){
imadaemi 2:7bf845f17396 325 //pc.printf("rate:%.3f\r\n", cam.get_rate());
imadaemi 1:d208f4e139a9 326 //xbee.printf("rate:%.3f\r\n", cam.get_rate());
imadaemi 1:d208f4e139a9 327
imadaemi 2:7bf845f17396 328 if(120 <= timer.read() - phase5_time){
imadaemi 2:7bf845f17396 329 cansat_phase = 6;
imadaemi 2:7bf845f17396 330 cansat_goal = 1;
imadaemi 2:7bf845f17396 331 Goal = 1;
imadaemi 2:7bf845f17396 332 }
imadaemi 2:7bf845f17396 333
imadaemi 1:d208f4e139a9 334 percent = cam.get_rate();
imadaemi 1:d208f4e139a9 335
imadaemi 1:d208f4e139a9 336 if(percent >= 0.1){
imadaemi 1:d208f4e139a9 337 color_mode = 2;
imadaemi 1:d208f4e139a9 338 //pc.printf("Goal!!!");
imadaemi 1:d208f4e139a9 339 //xbee.printf("Goal!!!");
imadaemi 2:7bf845f17396 340 //tick_data_status.detach();
imadaemi 2:7bf845f17396 341 //tick_data.detach();
imadaemi 2:7bf845f17396 342 cansat_phase = 6;
imadaemi 2:7bf845f17396 343 cansat_goal = 1;
imadaemi 1:d208f4e139a9 344 Goal = 1;
imadaemi 1:d208f4e139a9 345 }else if(percent >=0.001){
imadaemi 1:d208f4e139a9 346 color_mode = 1;
imadaemi 1:d208f4e139a9 347 direction_mode = 2;
imadaemi 1:d208f4e139a9 348 //pc.printf("Red!!!");
imadaemi 1:d208f4e139a9 349 //xbee.printf("Red!!!");
imadaemi 1:d208f4e139a9 350
imadaemi 1:d208f4e139a9 351 MotorFront(1.0, 1.0, 1.0);
imadaemi 2:7bf845f17396 352 //MotorFront(0, 1.0, 0.3);
imadaemi 2:7bf845f17396 353 //MotorFront(0, 0, 1.0);
imadaemi 1:d208f4e139a9 354 }else{
imadaemi 1:d208f4e139a9 355 color_mode = 0;
imadaemi 1:d208f4e139a9 356 direction_mode = -1;
imadaemi 1:d208f4e139a9 357
imadaemi 1:d208f4e139a9 358 //pc.printf("No Red!!!");
imadaemi 1:d208f4e139a9 359 //xbee.printf("No Red!!!");
imadaemi 1:d208f4e139a9 360 MotorFront(1.0, 0.5, 1.0);
imadaemi 2:7bf845f17396 361 //MotorFront(0, 0, 1.0);
imadaemi 1:d208f4e139a9 362 }
imadaemi 2:7bf845f17396 363 MotorFront(0.1, 0.1, 2.0);
imadaemi 2:7bf845f17396 364 //MotorFront(0, 0, 2.0);
imadaemi 1:d208f4e139a9 365 }
imadaemi 2:7bf845f17396 366 MotorStop();
imadaemi 1:d208f4e139a9 367 pc.printf("Bye!!!\n");
imadaemi 2:7bf845f17396 368
imadaemi 1:d208f4e139a9 369
imadaemi 1:d208f4e139a9 370 }
imadaemi 1:d208f4e139a9 371 /*
imadaemi 1:d208f4e139a9 372 void on_data_received()
imadaemi 1:d208f4e139a9 373 {
imadaemi 0:f3a52599183f 374 char data = xbee_raspi.getc();
imadaemi 0:f3a52599183f 375 xbee_raspi.printf("%c", data);
imadaemi 0:f3a52599183f 376 switch(data) {
imadaemi 0:f3a52599183f 377 case 0x81://ゴール判定
imadaemi 1:d208f4e139a9 378 //xbee_raspi.printf("red rate > 0.1\r\n");
imadaemi 0:f3a52599183f 379 break;
imadaemi 0:f3a52599183f 380 case 0x82://ゴール発見
imadaemi 1:d208f4e139a9 381 //xbee_raspi.printf("red rate > 0.01\r\n");
imadaemi 1:d208f4e139a9 382 //xbee.printf("Go Straight\r\n");
imadaemi 1:d208f4e139a9 383 MotorFront(1.0, 1.0, 1.0);
imadaemi 1:d208f4e139a9 384 MotorStop();
imadaemi 0:f3a52599183f 385 break;
imadaemi 0:f3a52599183f 386 case 0x83://ゴールが見えない
imadaemi 1:d208f4e139a9 387 //xbee_raspi.printf("cannot find red\r\n");
imadaemi 1:d208f4e139a9 388 //xbee.printf("Go to the Right360\r\n");
imadaemi 1:d208f4e139a9 389 MotorFront(1.0, 0, 1.0);
imadaemi 1:d208f4e139a9 390 MotorStop();
imadaemi 0:f3a52599183f 391 break;
imadaemi 0:f3a52599183f 392 }
imadaemi 0:f3a52599183f 393 }
imadaemi 1:d208f4e139a9 394 */
imadaemi 0:f3a52599183f 395 /***************************************************
imadaemi 0:f3a52599183f 396 センサのセットアップ
imadaemi 0:f3a52599183f 397 ***************************************************/
imadaemi 1:d208f4e139a9 398 void SetSensor()
imadaemi 1:d208f4e139a9 399 {
imadaemi 0:f3a52599183f 400 SetMpu9250();
imadaemi 0:f3a52599183f 401 SetLps22hb();
imadaemi 0:f3a52599183f 402 SetGPS();
imadaemi 0:f3a52599183f 403 }
imadaemi 1:d208f4e139a9 404
imadaemi 1:d208f4e139a9 405 void SetMpu9250() //MPU9250のセットアップ
imadaemi 1:d208f4e139a9 406 {
imadaemi 0:f3a52599183f 407 mpu9250_test = mpu9250.sensorTest();
imadaemi 0:f3a52599183f 408 mag_mpu9250_test = mpu9250.mag_sensorTest();
imadaemi 1:d208f4e139a9 409 if(mpu9250_test == true) {
imadaemi 0:f3a52599183f 410 pc.printf("MPU9250 OK!\r\n");
imadaemi 1:d208f4e139a9 411 //xbee.printf("MPU9250 OK!\r\n");
imadaemi 1:d208f4e139a9 412 } else {
imadaemi 0:f3a52599183f 413 pc.printf("MPU9250 NG...\r\n");
imadaemi 1:d208f4e139a9 414 //xbee.printf("MPU9250 NG...\r\n");
imadaemi 0:f3a52599183f 415 }
imadaemi 1:d208f4e139a9 416 if(mag_mpu9250_test == true) {
imadaemi 0:f3a52599183f 417 pc.printf("MPU9250 MAG OK!\r\n");
imadaemi 1:d208f4e139a9 418 //xbee.printf("MPU9250 MAG OK!\r\n");
imadaemi 1:d208f4e139a9 419 } else {
imadaemi 0:f3a52599183f 420 pc.printf("MPU9250 MAG NG...\r\n");
imadaemi 1:d208f4e139a9 421 //xbee.printf("MPU9250 MAG NG...\r\n");
imadaemi 0:f3a52599183f 422 }
imadaemi 0:f3a52599183f 423 mpu9250.setAcc(ACC_RANGE);
imadaemi 0:f3a52599183f 424 mpu9250.setGyro(GYRO_RANGE);
imadaemi 0:f3a52599183f 425 mpu9250.setOffset(0.528892327, -0.660206211, 0.757105891, -0.011691362, 0.025688783, 1.087885322, -159.750004, 121.425005, -392.700012);
imadaemi 0:f3a52599183f 426 }
imadaemi 0:f3a52599183f 427
imadaemi 1:d208f4e139a9 428 void SetLps22hb() //LPS22HBのセットアップ
imadaemi 1:d208f4e139a9 429 {
imadaemi 0:f3a52599183f 430 lps22hb.begin();
imadaemi 1:d208f4e139a9 431 if(lps22hb.test() == true) {
imadaemi 0:f3a52599183f 432 pc.printf("LPS22HB OK!\r\n");
imadaemi 1:d208f4e139a9 433 //xbee.printf("LPS22HB OK!\r\n");
imadaemi 1:d208f4e139a9 434 } else {
imadaemi 0:f3a52599183f 435 pc.printf("LPS22HB NG...\r\n");
imadaemi 1:d208f4e139a9 436 //xbee.printf("LPS22HB NG...\r\n");
imadaemi 0:f3a52599183f 437 }
imadaemi 0:f3a52599183f 438 }
imadaemi 0:f3a52599183f 439
imadaemi 1:d208f4e139a9 440 void SetGPS() //GPSのセットアップ
imadaemi 1:d208f4e139a9 441 {
imadaemi 0:f3a52599183f 442 // gps.changeGPSFreq(10); 入れるとエラーが出る
imadaemi 0:f3a52599183f 443 pc.printf("GPS OK!\r\n");
imadaemi 1:d208f4e139a9 444 //xbee.printf("GPS OK!\r\n");
imadaemi 0:f3a52599183f 445 }
imadaemi 0:f3a52599183f 446
imadaemi 0:f3a52599183f 447 /***************************************************
imadaemi 0:f3a52599183f 448 データの取得
imadaemi 0:f3a52599183f 449 ***************************************************/
imadaemi 1:d208f4e139a9 450 void GetData()
imadaemi 1:d208f4e139a9 451 {
imadaemi 0:f3a52599183f 452 GetMpu9250();
imadaemi 0:f3a52599183f 453 GetLps22hb();
imadaemi 0:f3a52599183f 454 GetGPS();
imadaemi 2:7bf845f17396 455 //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);
imadaemi 2:7bf845f17396 456 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);
imadaemi 0:f3a52599183f 457 }
imadaemi 0:f3a52599183f 458
imadaemi 1:d208f4e139a9 459 void GetStatus()
imadaemi 1:d208f4e139a9 460 {
imadaemi 2:7bf845f17396 461 xbee.printf("**************************************\r\n");
imadaemi 2:7bf845f17396 462 xbee.printf("Phase : %d\r\n", cansat_phase);
imadaemi 2:7bf845f17396 463 xbee.printf("Altitude_02 : %.3f\r\n", CalcAltitude_02());
imadaemi 2:7bf845f17396 464 xbee.printf("Driving Mode : %d\r\n", driving_mode);
imadaemi 2:7bf845f17396 465 xbee.printf("Direction : %d\r\n", direction_mode);
imadaemi 2:7bf845f17396 466 xbee.printf("Distance : %.2f\r\n", cansat_distance);
imadaemi 2:7bf845f17396 467 xbee.printf("lat : %.2f\r\n", lat);
imadaemi 2:7bf845f17396 468 xbee.printf("lat_0 : %.2f\r\n", lat_0);
imadaemi 2:7bf845f17396 469 xbee.printf("lon : %.2f\r\n", lon);
imadaemi 2:7bf845f17396 470 xbee.printf("lon_0 : %.2f\r\n", lon_0);
imadaemi 2:7bf845f17396 471 xbee.printf("Color Percent: %f\r\n", percent);
imadaemi 2:7bf845f17396 472 xbee.printf("Color : %d\r\n", color_mode);
imadaemi 2:7bf845f17396 473 xbee.printf("Phase5 Time : %f\r\n", timer.read() - phase5_time);
imadaemi 2:7bf845f17396 474 xbee.printf("Goal : %d\r\n", cansat_goal);
imadaemi 2:7bf845f17396 475 xbee.printf("Counter : %d\r\n", count_data);
imadaemi 2:7bf845f17396 476 xbee.printf("**************************************\r\n");
imadaemi 2:7bf845f17396 477 count_data++;
imadaemi 1:d208f4e139a9 478 }
imadaemi 1:d208f4e139a9 479
imadaemi 1:d208f4e139a9 480 void GetMpu9250()
imadaemi 1:d208f4e139a9 481 {
imadaemi 0:f3a52599183f 482 mpu9250.getAll(imu, mag);
imadaemi 0:f3a52599183f 483 //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]);
imadaemi 0:f3a52599183f 484 //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]);
imadaemi 0:f3a52599183f 485 //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]);
imadaemi 0:f3a52599183f 486 }
imadaemi 0:f3a52599183f 487
imadaemi 1:d208f4e139a9 488 void GetLps22hb()
imadaemi 1:d208f4e139a9 489 {
imadaemi 0:f3a52599183f 490 lps22hb.read_press(&press);
imadaemi 0:f3a52599183f 491 lps22hb.read_temp(&temp);
imadaemi 0:f3a52599183f 492 //pc.printf("%f\t%f\t",press, temp);
imadaemi 0:f3a52599183f 493 }
imadaemi 0:f3a52599183f 494
imadaemi 1:d208f4e139a9 495 void GetGPS()
imadaemi 1:d208f4e139a9 496 {
imadaemi 0:f3a52599183f 497 lat = gps.Latitude();
imadaemi 0:f3a52599183f 498 lon = gps.Longitude();
imadaemi 0:f3a52599183f 499 height = gps.Height();
imadaemi 0:f3a52599183f 500 //pc.printf("%f\t%f\t%f\t", lat, lon, height);
imadaemi 0:f3a52599183f 501 //xbee.printf("%f\t%f\t%f\t", lat, lon, height);
imadaemi 0:f3a52599183f 502 //openlog.printf("%f\t%f\t%f\t", lat, lon, height);
imadaemi 0:f3a52599183f 503 }
imadaemi 0:f3a52599183f 504
imadaemi 0:f3a52599183f 505 /***************************************************
imadaemi 0:f3a52599183f 506 制御計算
imadaemi 0:f3a52599183f 507 ***************************************************/
imadaemi 1:d208f4e139a9 508 void CalculateAltitude()
imadaemi 1:d208f4e139a9 509 {
imadaemi 0:f3a52599183f 510 lps22hb.read_press(&press);
imadaemi 1:d208f4e139a9 511 lps22hb.read_temp(&temp);
imadaemi 0:f3a52599183f 512 altitude_ground = (powf(SEA_LEVEL_PRESS / CURRENT_LOCATION_PRESS, 1/5.257) - 1) * (CURRENT_LOCATION_TEMP + 273.15) / 0.0065;
imadaemi 0:f3a52599183f 513 altitude = (powf(SEA_LEVEL_PRESS / press, 1/5.257) - 1) * (temp + 273.15) / 0.0065 - altitude_ground;
imadaemi 0:f3a52599183f 514 pc.printf("%f\t\r\n",altitude);
imadaemi 0:f3a52599183f 515 }
imadaemi 0:f3a52599183f 516
imadaemi 2:7bf845f17396 517 float CalcAltitude_02(){
imadaemi 2:7bf845f17396 518 lps22hb.read_press(&press);
imadaemi 2:7bf845f17396 519 lps22hb.read_temp(&temp);
imadaemi 2:7bf845f17396 520 altitude_02 = (CURRENT_LOCATION_TEMP + 273.15)/0.0065*(1 - powf(press/CURRENT_LOCATION_PRESS, 287 * 0.0065/9.80665));
imadaemi 2:7bf845f17396 521 //pc.printf("%f\t%f\t\r\n",press, altitude_02);
imadaemi 2:7bf845f17396 522 return altitude_02;
imadaemi 2:7bf845f17396 523 }
imadaemi 2:7bf845f17396 524
imadaemi 1:d208f4e139a9 525 void FlightPin()
imadaemi 1:d208f4e139a9 526 {
imadaemi 0:f3a52599183f 527 //pc.printf("Hello FlightPin!\r\n");
imadaemi 1:d208f4e139a9 528 while(cansat_phase == 2) {
imadaemi 1:d208f4e139a9 529 if(flightpin == 1) {
imadaemi 1:d208f4e139a9 530 break;
imadaemi 0:f3a52599183f 531 }
imadaemi 0:f3a52599183f 532 }
imadaemi 1:d208f4e139a9 533 //pc.printf("Goodbye FlightPin!\r\n");
imadaemi 0:f3a52599183f 534 }
imadaemi 0:f3a52599183f 535
imadaemi 1:d208f4e139a9 536 void CutNichrome(float nich_wait)
imadaemi 1:d208f4e139a9 537 {
imadaemi 0:f3a52599183f 538 NichromeOn();
imadaemi 0:f3a52599183f 539 wait(nich_wait);
imadaemi 0:f3a52599183f 540 NichromeOff();
imadaemi 0:f3a52599183f 541 }
imadaemi 0:f3a52599183f 542
imadaemi 1:d208f4e139a9 543 void NichromeOn()
imadaemi 1:d208f4e139a9 544 {
imadaemi 0:f3a52599183f 545 pin = 1;
imadaemi 0:f3a52599183f 546 nich_status = true;
imadaemi 0:f3a52599183f 547 }
imadaemi 0:f3a52599183f 548
imadaemi 1:d208f4e139a9 549 void NichromeOff()
imadaemi 1:d208f4e139a9 550 {
imadaemi 0:f3a52599183f 551 pin = 0;
imadaemi 0:f3a52599183f 552 nich_status = false;
imadaemi 1:d208f4e139a9 553 }
imadaemi 0:f3a52599183f 554
imadaemi 1:d208f4e139a9 555 void CalcDirection(float lat, float lon, float lat_f, float lon_f) //ヒュベニの公式を使いたい
imadaemi 1:d208f4e139a9 556 {
imadaemi 0:f3a52599183f 557 float dlat_g = (LAT_GOAL - lat) * (PI / 180);
imadaemi 0:f3a52599183f 558 float dlon_g = (LON_GOAL - lon) * (PI / 180);
imadaemi 0:f3a52599183f 559 float direc_g = atan2(dlat_g, dlon_g) * (180 / PI);
imadaemi 0:f3a52599183f 560
imadaemi 0:f3a52599183f 561 float dlat = (lat - lat_f) * (PI / 180);
imadaemi 0:f3a52599183f 562 float dlon = (lon - lon_f) * (PI / 180);
imadaemi 0:f3a52599183f 563 float direc = atan2(dlat, dlon) * (180 / PI);
imadaemi 0:f3a52599183f 564
imadaemi 0:f3a52599183f 565 cansat_direction = direc_g - direc;
imadaemi 0:f3a52599183f 566 }
imadaemi 0:f3a52599183f 567
imadaemi 1:d208f4e139a9 568 void CalcDistance(float lat, float lon)
imadaemi 1:d208f4e139a9 569 {
imadaemi 0:f3a52599183f 570 float dlat_distance = R * (LAT_GOAL - lat) * (PI / 180);
imadaemi 0:f3a52599183f 571 float dlon_distance = R * (LON_GOAL - lon) * (PI / 180) * cos(lat * (PI / 180));
imadaemi 0:f3a52599183f 572 float distance_squared = dlat_distance * dlat_distance + dlon_distance * dlon_distance;
imadaemi 0:f3a52599183f 573 cansat_distance = sqrt(distance_squared);
imadaemi 0:f3a52599183f 574 }
imadaemi 0:f3a52599183f 575
imadaemi 0:f3a52599183f 576 /***************************************************
imadaemi 0:f3a52599183f 577 モーター制御
imadaemi 0:f3a52599183f 578 ***************************************************/
imadaemi 1:d208f4e139a9 579 void MotorFront(float f_left,float f_right,float f_wait)
imadaemi 1:d208f4e139a9 580 {
imadaemi 0:f3a52599183f 581 motor_left26 = 0;//正転
imadaemi 0:f3a52599183f 582 motor_left25 = f_left;
imadaemi 0:f3a52599183f 583 motor_right24 = f_right;
imadaemi 0:f3a52599183f 584 motor_right23 = 0;
imadaemi 0:f3a52599183f 585 wait(f_wait);
imadaemi 0:f3a52599183f 586 }
imadaemi 0:f3a52599183f 587
imadaemi 1:d208f4e139a9 588 void MotorBack(float b_left,float b_right,float b_wait)
imadaemi 1:d208f4e139a9 589 {
imadaemi 0:f3a52599183f 590 motor_left26 = b_left;//逆転
imadaemi 0:f3a52599183f 591 motor_left25 = 0;
imadaemi 0:f3a52599183f 592 motor_right24 = 0;
imadaemi 0:f3a52599183f 593 motor_right23 = b_right;
imadaemi 0:f3a52599183f 594 wait(b_wait);
imadaemi 0:f3a52599183f 595 }
imadaemi 0:f3a52599183f 596
imadaemi 1:d208f4e139a9 597 void MotorStop()
imadaemi 1:d208f4e139a9 598 {
imadaemi 0:f3a52599183f 599 motor_left26 = 0;//ストップ
imadaemi 0:f3a52599183f 600 motor_left25 = 0;
imadaemi 0:f3a52599183f 601 motor_right24 = 0;
imadaemi 0:f3a52599183f 602 motor_right23 = 0;
imadaemi 0:f3a52599183f 603 }