QQQ / Mbed 2 deprecated qqq_izu_main_test

20.09.29_main program test

Dependencies:   mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt

main.cpp@1:d208f4e139a9, 2020-09-29 (annotated)

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