QQQ / Mbed 2 deprecated qqq_izu_main_test

20.09.29_main program test

Dependencies:   mbed mpu9250_i2c QQQCAM PQLPS22HB GPS_interrupt

main.cpp@3:b48cac06f913, 2020-12-01 (annotated)

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