cansat-d_2018 / Mbed 2 deprecated cansat_main_new_3

otamesi

Dependencies:   mbed

main.cpp@41:7c537a922510, 2019-02-16 (annotated)

Committer:
seangshim
Date:
Sat Feb 16 05:40:24 2019 +0000
Revision:
41:7c537a922510
Parent:
40:d0b0d2a26cac 
nei

Who changed what in which revision?

UserRevisionLine numberNew contents of line
seangshim 0:a01fda36fde8 1 #include "mbed.h"
seangshim 0:a01fda36fde8 2 #include "gps.h"
seangshim 0:a01fda36fde8 3 #include "ultrasonic.h"
seangshim 0:a01fda36fde8 4 #include "motordriver.h"
seangshim 6:574d9a6253c7 5 #include "HMC5883L.h"
seangshim 8:d41f5d7d2aa5 6 #include "MPU6050.h"
seangshim 6:574d9a6253c7 7
seangshim 27:b4922048ab11 8
seangshim 38:d3b7fda44f72 9 PwmOut motorSpeedR(p26);
seangshim 38:d3b7fda44f72 10 PwmOut motorSpeedL(p25);
seangshim 35:40df2f91cea9 11 DigitalOut motor1Dir1(p19);
seangshim 35:40df2f91cea9 12 DigitalOut motor1Dir2(p20);
seangshim 35:40df2f91cea9 13 DigitalOut motor2Dir1(p16);
seangshim 35:40df2f91cea9 14 DigitalOut motor2Dir2(p17);
seangshim 27:b4922048ab11 15
seangshim 8:d41f5d7d2aa5 16
seangshim 8:d41f5d7d2aa5 17 Serial pc(USBTX, USBRX); //地磁気センサー,GPS
seangshim 9:fb19a93df88f 18 GPS gps(p28, p27); //GPS
seangshim 9:fb19a93df88f 19 HMC5883L compass(p9, p10); //地磁気センサー
seangshim 9:fb19a93df88f 20
seangshim 38:d3b7fda44f72 21 MPU6050 mpu(p9, p10); //(SDA,SCL) 加速度センサー*/
seangshim 9:fb19a93df88f 22
seangshim 33:8d3757a0cd93 23 DigitalIn flight(p23); //フライトピン
seangshim 33:8d3757a0cd93 24 DigitalOut SW(p22); //トリガー用
seangshim 33:8d3757a0cd93 25 DigitalOut FET(p21); //FET
seangshim 1:10af8aaa5b40 26
seangshim 1:10af8aaa5b40 27 InterruptIn button1(p15); //フォトインタラプタ
seangshim 2:37d831f82840 28 InterruptIn button2(p18);
seangshim 1:10af8aaa5b40 29 DigitalIn test(p15); //ここでピン15からの電圧の値、つまりフォトインタラプタが何か遮るものを検知すればハイの1を返して、
seangshim 1:10af8aaa5b40 30 //何もないつまりスリットの部分ではローの0を返す。それを変数testに代入している
seangshim 2:37d831f82840 31 DigitalIn test2(p18);
seangshim 1:10af8aaa5b40 32
seangshim 0:a01fda36fde8 33
seangshim 38:d3b7fda44f72 34 //Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター
seangshim 38:d3b7fda44f72 35 //Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
seangshim 0:a01fda36fde8 36
seangshim 0:a01fda36fde8 37 void dist(int distance)
seangshim 0:a01fda36fde8 38 {
seangshim 0:a01fda36fde8 39 //put code here to happen when the distance is changed
seangshim 0:a01fda36fde8 40 printf("Distance changed to %dmm\r\n", distance);
seangshim 0:a01fda36fde8 41 }
seangshim 0:a01fda36fde8 42
seangshim 1:10af8aaa5b40 43 ultrasonic mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 超音波センサー
seangshim 0:a01fda36fde8 44 //have updates every .1 seconds and a timeout after 1
seangshim 0:a01fda36fde8 45 //second, and call dist when the distance changes
seangshim 27:b4922048ab11 46
seangshim 38:d3b7fda44f72 47 void motorForwardR(void);
seangshim 38:d3b7fda44f72 48 void motorStopR(void);
seangshim 38:d3b7fda44f72 49 void motorReverseR(void);
seangshim 0:a01fda36fde8 50
seangshim 38:d3b7fda44f72 51 void motorForwardL(void);
seangshim 38:d3b7fda44f72 52 void motorStopL(void);
seangshim 38:d3b7fda44f72 53 void motorReverseL(void);
seangshim 0:a01fda36fde8 54
seangshim 35:40df2f91cea9 55 LocalFileSystem local("local"); // Create the local filesystem under the name "local"　　 データ保存
seangshim 0:a01fda36fde8 56
seangshim 27:b4922048ab11 57 float culculate_distance_3(float a,float b);
seangshim 27:b4922048ab11 58
seangshim 0:a01fda36fde8 59 int main() {
seangshim 0:a01fda36fde8 60
seangshim 27:b4922048ab11 61 printf("cansat start\r\n");
seangshim 40:d0b0d2a26cac 62 flight==1;
seangshim 33:8d3757a0cd93 63 FET = 0;
seangshim 33:8d3757a0cd93 64 SW = 1; //p23をhigh(3.3V)にする。
seangshim 38:d3b7fda44f72 65 while(1) {
seangshim 17:9bc130ebb5ed 66 if(flight==1) {
seangshim 17:9bc130ebb5ed 67 wait(10);
seangshim 38:d3b7fda44f72 68 printf("mada\r\n");
seangshim 17:9bc130ebb5ed 69 }
seangshim 17:9bc130ebb5ed 70
seangshim 17:9bc130ebb5ed 71 else{
seangshim 33:8d3757a0cd93 72 if(flight==1) {
seangshim 33:8d3757a0cd93 73 wait(10);
seangshim 38:d3b7fda44f72 74 printf("madamada\r\n");
seangshim 33:8d3757a0cd93 75 }
seangshim 33:8d3757a0cd93 76 else{
seangshim 38:d3b7fda44f72 77 printf("yattar\r\n");
seangshim 35:40df2f91cea9 78 FET = 0; //FET、ニクロム線切断
seangshim 17:9bc130ebb5ed 79 wait(20);
seangshim 1:10af8aaa5b40 80 FET = 1;
seangshim 17:9bc130ebb5ed 81 wait(12);
seangshim 17:9bc130ebb5ed 82 FET = 0;
seangshim 17:9bc130ebb5ed 83 wait(10);
seangshim 17:9bc130ebb5ed 84 FET = 1;
seangshim 17:9bc130ebb5ed 85 wait(12);
seangshim 35:40df2f91cea9 86 FET = 0;
seangshim 38:d3b7fda44f72 87 SW = 0; //p23をlow(0V)にする。
seangshim 17:9bc130ebb5ed 88 break;
seangshim 17:9bc130ebb5ed 89 }
seangshim 33:8d3757a0cd93 90 }
seangshim 40:d0b0d2a26cac 91 }
seangshim 0:a01fda36fde8 92
seangshim 38:d3b7fda44f72 93 motorStopR();
seangshim 38:d3b7fda44f72 94 motorStopL();
seangshim 38:d3b7fda44f72 95
seangshim 40:d0b0d2a26cac 96 wait(20); //確認用//デフォ20秒
seangshim 19:8ad7dcfef11f 97
seangshim 35:40df2f91cea9 98 // FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 32:c522d463c4fa 99
seangshim 35:40df2f91cea9 100 printf("GPS start\r\n");
seangshim 27:b4922048ab11 101 FILE *fp = fopen("/local/gps,foto.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 40:d0b0d2a26cac 102 fprintf(fp, "GPS Start\r\n");
seangshim 0:a01fda36fde8 103 int n;
seangshim 1:10af8aaa5b40 104 for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
seangshim 0:a01fda36fde8 105 {
seangshim 9:fb19a93df88f 106 printf("gps for\r\n");
seangshim 0:a01fda36fde8 107 if(gps.getgps()) //現在地取得
seangshim 0:a01fda36fde8 108 fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
seangshim 0:a01fda36fde8 109
seangshim 0:a01fda36fde8 110 else
seangshim 0:a01fda36fde8 111 fprintf(fp,"No data\r\n");//データ取得に失敗した場合
seangshim 0:a01fda36fde8 112
seangshim 0:a01fda36fde8 113 wait(1);
seangshim 1:10af8aaa5b40 114
seangshim 1:10af8aaa5b40 115 printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
seangshim 1:10af8aaa5b40 116
seangshim 0:a01fda36fde8 117 }
seangshim 27:b4922048ab11 118 fprintf(fp,"GPS finish\r\n");
seangshim 35:40df2f91cea9 119 // fclose(fp); // GPSの測定終了 */
seangshim 28:5e21ce413558 120
seangshim 38:d3b7fda44f72 121 motorSpeedR.period_ms(4); //モーター調節
seangshim 41:7c537a922510 122 motorSpeedR = 0.655;
seangshim 38:d3b7fda44f72 123 motorSpeedL.period_ms(4); //モーター調節
seangshim 41:7c537a922510 124 motorSpeedL = 0.755;
seangshim 9:fb19a93df88f 125
394 10:280a25bcc8bb 126 compass.init(); //地磁気センサー動作
seangshim 16:44c763c32b0d 127
394 10:280a25bcc8bb 128 int i;
seangshim 38:d3b7fda44f72 129 for(i=0;i<20;i++) //地磁気測定//デフォ20
394 10:280a25bcc8bb 130 {
394 10:280a25bcc8bb 131 pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
seangshim 29:05b390de92ed 132 fprintf(fp,"raw=\r\n");
seangshim 29:05b390de92ed 133 fprintf(fp,"%lf\r\n",compass.getHeadingXYDeg());
394 10:280a25bcc8bb 134 wait(0.5);
seangshim 16:44c763c32b0d 135 }
394 10:280a25bcc8bb 136 float mc1,mc2;
394 20:e78bffff80ee 137 mc1=1.0;
394 20:e78bffff80ee 138 mc2=1.0;
394 10:280a25bcc8bb 139 //地磁気センサのキャリブレーション
seangshim 38:d3b7fda44f72 140 motorForwardL(); //車体を時計回りに３秒回転
seangshim 38:d3b7fda44f72 141 motorReverseR();
394 30:3bcf14e8dd63 142 wait(1.6);
394 10:280a25bcc8bb 143
seangshim 38:d3b7fda44f72 144 motorStopR();
seangshim 38:d3b7fda44f72 145 motorStopL();
394 10:280a25bcc8bb 146 wait(1);
394 10:280a25bcc8bb 147
seangshim 38:d3b7fda44f72 148 motorReverseL(); //車体を反時計回りに３秒回
seangshim 38:d3b7fda44f72 149 motorForwardR();
394 30:3bcf14e8dd63 150 wait(1.6);
394 10:280a25bcc8bb 151
seangshim 38:d3b7fda44f72 152 motorStopR();
seangshim 38:d3b7fda44f72 153 motorStopL();
394 10:280a25bcc8bb 154 wait(1);
394 10:280a25bcc8bb 155 printf("compass carriblation\r\n"); //キャリブレーション終了
394 10:280a25bcc8bb 156
seangshim 38:d3b7fda44f72 157 //float mcn1,mcn2;
seangshim 40:d0b0d2a26cac 158 double heading,wx,wy,wz;
seangshim 38:d3b7fda44f72 159 // mcn1=1.0;
seangshim 38:d3b7fda44f72 160 // mcn2=1.0;
seangshim 38:d3b7fda44f72 161
seangshim 38:d3b7fda44f72 162 heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
394 30:3bcf14e8dd63 163 if(90<heading<267.5){
seangshim 38:d3b7fda44f72 164 printf("right\r\n");
seangshim 38:d3b7fda44f72 165 motorForwardL();//右回転
seangshim 38:d3b7fda44f72 166 motorReverseR();
seangshim 40:d0b0d2a26cac 167 wx=(270-heading)*0.004448;
seangshim 40:d0b0d2a26cac 168 wait(wx); //角度のずれ*1度回転するのにかかる時間
seangshim 38:d3b7fda44f72 169 motorStopR();
seangshim 38:d3b7fda44f72 170 motorStopL();
394 10:280a25bcc8bb 171 wait(1);
seangshim 38:d3b7fda44f72 172 }
seangshim 38:d3b7fda44f72 173 else if(0<=heading<=90.0){
seangshim 38:d3b7fda44f72 174 printf("left\r\n");
seangshim 40:d0b0d2a26cac 175 motorForwardL();//左回転
seangshim 40:d0b0d2a26cac 176 motorReverseR();
seangshim 40:d0b0d2a26cac 177 wy=(270-heading)*0.004448;
seangshim 40:d0b0d2a26cac 178 wait(wy);
seangshim 38:d3b7fda44f72 179 motorStopR();
seangshim 38:d3b7fda44f72 180 motorStopL();
394 30:3bcf14e8dd63 181 wait(1);
seangshim 38:d3b7fda44f72 182 }
seangshim 38:d3b7fda44f72 183 else if(272.5<heading<360){
seangshim 38:d3b7fda44f72 184 printf("left\r\n");
seangshim 40:d0b0d2a26cac 185 motorForwardL();//左回転
seangshim 40:d0b0d2a26cac 186 motorReverseR();
seangshim 40:d0b0d2a26cac 187 wz=(360-heading)*0.004448;
seangshim 40:d0b0d2a26cac 188 wait(wz);
seangshim 38:d3b7fda44f72 189 motorStopR();
seangshim 38:d3b7fda44f72 190 motorStopL();
394 10:280a25bcc8bb 191 wait(1);
seangshim 38:d3b7fda44f72 192 }
seangshim 38:d3b7fda44f72 193 else{
394 20:e78bffff80ee 194 wait(5);
394 10:280a25bcc8bb 195 }
394 10:280a25bcc8bb 196 printf("searchN\r\n"); //機体が北を向く
seangshim 38:d3b7fda44f72 197 wait(2);
seangshim 0:a01fda36fde8 198
seangshim 0:a01fda36fde8 199 mu.startUpdates();//start mesuring the distance(超音波センサー)
seangshim 3:c1e0db4832b7 200 int distance;
seangshim 0:a01fda36fde8 201
seangshim 13:b884f5960fbf 202 int flag=0,flag2=0; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
seangshim 1:10af8aaa5b40 203 //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
seangshim 1:10af8aaa5b40 204 //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
seangshim 2:37d831f82840 205 float rightrun; //変数runをフロートで型づけする
seangshim 39:a0c63ae63c99 206 float leftrun;
seangshim 4:8b52fd631b32 207 rightrun=0.0;
seangshim 39:a0c63ae63c99 208 leftrun=0.0;
seangshim 35:40df2f91cea9 209
seangshim 27:b4922048ab11 210 int accel[3];//accelを3つの配列で定義。*/
seangshim 40:d0b0d2a26cac 211 int gyro[3];
seangshim 27:b4922048ab11 212
seangshim 27:b4922048ab11 213 int tt=0;
seangshim 27:b4922048ab11 214 float run=0;
seangshim 35:40df2f91cea9 215 fprintf(fp, "x,y,z,lefttrun2,rightrun\r\n");
seangshim 35:40df2f91cea9 216
seangshim 38:d3b7fda44f72 217 int k=0;//中間地点角度判定プログラムを１回しか通さないためのフラグ
seangshim 3:c1e0db4832b7 218 while(1)
seangshim 3:c1e0db4832b7 219 {
seangshim 3:c1e0db4832b7 220 distance=mu.getCurrentDistance();
seangshim 3:c1e0db4832b7 221 printf("%d\r\n",distance);
seangshim 8:d41f5d7d2aa5 222
seangshim 8:d41f5d7d2aa5 223 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
seangshim 40:d0b0d2a26cac 224 int x = accel[0];//x軸方向の加速度
seangshim 40:d0b0d2a26cac 225 int y = accel[1];//y軸方向の加速度
seangshim 40:d0b0d2a26cac 226 int z = accel[2] ;//z軸方向の加速度
seangshim 40:d0b0d2a26cac 227 mpu.readGyroData(gyro);//加速度の値をaccel[3]に代入
seangshim 40:d0b0d2a26cac 228 int gx = gyro[0];//x軸方向の加速度
seangshim 40:d0b0d2a26cac 229 int gy = gyro[1];//y軸方向の加速度
seangshim 40:d0b0d2a26cac 230 int gz = gyro[2];//z軸方向の加速度
seangshim 17:9bc130ebb5ed 231
seangshim 38:d3b7fda44f72 232 printf("x=%d,y=%d,z=%d\r\n",x,y,z); //加速度の表示
seangshim 3:c1e0db4832b7 233
seangshim 8:d41f5d7d2aa5 234 printf("%d\r\n", test.read()); //フォトインタラプタ
seangshim 3:c1e0db4832b7 235 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 236 if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
seangshim 1:10af8aaa5b40 237 flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
seangshim 3:c1e0db4832b7 238 printf("test.read if\r\n");
seangshim 1:10af8aaa5b40 239 }
seangshim 1:10af8aaa5b40 240 else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
seangshim 1:10af8aaa5b40 241 flag = 0; //まずこれでスイッチを0にして入れる。
seangshim 40:d0b0d2a26cac 242 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も56.5mmを加算しつづけるということがなくなる
seangshim 40:d0b0d2a26cac 243 rightrun += 113; //総距離runに113mmを加算する(タイヤの全周452mm)
seangshim 3:c1e0db4832b7 244 printf("test.read else\r\n");
seangshim 2:37d831f82840 245 }
seangshim 2:37d831f82840 246 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 247 flag2 = 1;
seangshim 3:c1e0db4832b7 248 printf("test2.read if\r\n");
seangshim 1:10af8aaa5b40 249 }
seangshim 2:37d831f82840 250 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 251 flag2 = 0;
seangshim 40:d0b0d2a26cac 252 leftrun += 113;
seangshim 3:c1e0db4832b7 253 printf("test2.read else\r\n");
seangshim 2:37d831f82840 254 }
seangshim 40:d0b0d2a26cac 255 printf("%f %g rot ", leftrun,leftrun/452);
seangshim 40:d0b0d2a26cac 256 printf("%f %g rot\r\n", rightrun,rightrun/452);
seangshim 39:a0c63ae63c99 257 run=culculate_distance_3(rightrun,leftrun);
seangshim 40:d0b0d2a26cac 258 if(run >= 30000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 40:d0b0d2a26cac 259 break; //つまりゴールについたらこのループからぬける
seangshim 40:d0b0d2a26cac 260 }
seangshim 40:d0b0d2a26cac 261 else if(15000< run && run<15050 && k!=1){//中間地点で北を向きなおす
seangshim 38:d3b7fda44f72 262 motorStopR();
seangshim 38:d3b7fda44f72 263 motorStopL();
394 31:1ee80995740a 264 wait(1);
seangshim 38:d3b7fda44f72 265 heading=compass.getHeadingXYDeg();//コンパスの値をもう一度headingに格納
seangshim 38:d3b7fda44f72 266 wait(2);
seangshim 38:d3b7fda44f72 267 k=1;
seangshim 38:d3b7fda44f72 268
seangshim 40:d0b0d2a26cac 269 heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
seangshim 38:d3b7fda44f72 270 if(90<heading<267.5){
seangshim 40:d0b0d2a26cac 271 printf("right\r\n");
seangshim 38:d3b7fda44f72 272 motorForwardL();//右回転
seangshim 38:d3b7fda44f72 273 motorReverseR();
seangshim 40:d0b0d2a26cac 274 wx=(270-heading)*0.004448;
seangshim 40:d0b0d2a26cac 275 wait(wx); //角度のずれ*1度回転するのにかかる時間
seangshim 38:d3b7fda44f72 276 motorStopR();
seangshim 38:d3b7fda44f72 277 motorStopL();
394 30:3bcf14e8dd63 278 wait(1);
seangshim 40:d0b0d2a26cac 279 }
seangshim 40:d0b0d2a26cac 280 else if(0<=heading<=90.0){
seangshim 40:d0b0d2a26cac 281 printf("left\r\n");
seangshim 40:d0b0d2a26cac 282 motorForwardL();//左回転
seangshim 40:d0b0d2a26cac 283 motorReverseR();
seangshim 40:d0b0d2a26cac 284 wy=(270-heading)*0.004448;
seangshim 40:d0b0d2a26cac 285 wait(wy);
seangshim 38:d3b7fda44f72 286 motorStopR();
seangshim 38:d3b7fda44f72 287 motorStopL();
394 30:3bcf14e8dd63 288 wait(1);
seangshim 40:d0b0d2a26cac 289 }
seangshim 40:d0b0d2a26cac 290 else if(272.5<heading<360){
seangshim 40:d0b0d2a26cac 291 printf("left\r\n");
seangshim 40:d0b0d2a26cac 292 motorForwardL();//左回転
seangshim 40:d0b0d2a26cac 293 motorReverseR();
seangshim 40:d0b0d2a26cac 294 wz=(360-heading)*0.004448;
seangshim 40:d0b0d2a26cac 295 wait(wz);
seangshim 40:d0b0d2a26cac 296 motorStopR();
seangshim 40:d0b0d2a26cac 297 motorStopL();
seangshim 40:d0b0d2a26cac 298 wait(1);
seangshim 40:d0b0d2a26cac 299 }
seangshim 40:d0b0d2a26cac 300 else{
394 30:3bcf14e8dd63 301 wait(5);
394 30:3bcf14e8dd63 302 }
394 30:3bcf14e8dd63 303 printf("searchN\r\n"); //機体が北を向く
394 30:3bcf14e8dd63 304 }
seangshim 40:d0b0d2a26cac 305
seangshim 0:a01fda36fde8 306
seangshim 27:b4922048ab11 307 /* if(difference >= 0.3)
seangshim 19:8ad7dcfef11f 308 {
seangshim 19:8ad7dcfef11f 309 break;
seangshim 27:b4922048ab11 310 } */
seangshim 19:8ad7dcfef11f 311
seangshim 38:d3b7fda44f72 312 motorForwardL(); //通常走行
seangshim 38:d3b7fda44f72 313 motorForwardR();
seangshim 1:10af8aaa5b40 314 //Do something else here
seangshim 1:10af8aaa5b40 315 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
seangshim 1:10af8aaa5b40 316 //the class checks if dist needs to be called.
seangshim 0:a01fda36fde8 317
seangshim 9:fb19a93df88f 318 wait(0.01);
seangshim 0:a01fda36fde8 319
seangshim 27:b4922048ab11 320
seangshim 40:d0b0d2a26cac 321 fprintf(fp,"%5d, %5d, %5d, %5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z,gx,gy,gz, leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力
seangshim 27:b4922048ab11 322
seangshim 38:d3b7fda44f72 323 if(450 < distance && distance < 550) //障害物発見
seangshim 0:a01fda36fde8 324 {
seangshim 0:a01fda36fde8 325
seangshim 0:a01fda36fde8 326 printf("if success!\r\n");
seangshim 0:a01fda36fde8 327
seangshim 0:a01fda36fde8 328 float ms1,ms2,msj1,msj2;
seangshim 1:10af8aaa5b40 329 ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
seangshim 1:10af8aaa5b40 330 ms2=1.0;
seangshim 0:a01fda36fde8 331
seangshim 12:2f0c841e6078 332 msj1=1.0; //回転の時
seangshim 12:2f0c841e6078 333 msj2=1.0;
seangshim 0:a01fda36fde8 334
seangshim 38:d3b7fda44f72 335 motorStopR();
seangshim 38:d3b7fda44f72 336 motorStopL();
seangshim 1:10af8aaa5b40 337 wait(2);
seangshim 0:a01fda36fde8 338 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 339
seangshim 38:d3b7fda44f72 340 motorForwardL(); //機体を時計回りに９０度回転
seangshim 38:d3b7fda44f72 341 motorReverseR();
seangshim 40:d0b0d2a26cac 342 wait(1);
seangshim 0:a01fda36fde8 343 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 344
seangshim 38:d3b7fda44f72 345 motorStopR();
seangshim 38:d3b7fda44f72 346 motorStopL();
seangshim 3:c1e0db4832b7 347 wait(2);
seangshim 3:c1e0db4832b7 348 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 349
seangshim 38:d3b7fda44f72 350 motorForwardL(); //直進
seangshim 38:d3b7fda44f72 351 motorForwardR();
seangshim 40:d0b0d2a26cac 352 wait(3);
seangshim 0:a01fda36fde8 353 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 354
seangshim 38:d3b7fda44f72 355 motorStopR();
seangshim 38:d3b7fda44f72 356 motorStopL();
seangshim 3:c1e0db4832b7 357 wait(2);
seangshim 3:c1e0db4832b7 358 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 359
seangshim 38:d3b7fda44f72 360 motorReverseL(); //機体を反時計回りに９０度回転
seangshim 38:d3b7fda44f72 361 motorForwardR();
seangshim 40:d0b0d2a26cac 362 wait(1);
seangshim 0:a01fda36fde8 363 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 364
seangshim 38:d3b7fda44f72 365 motorStopR();
seangshim 38:d3b7fda44f72 366 motorStopL();
seangshim 3:c1e0db4832b7 367 wait(2);
seangshim 3:c1e0db4832b7 368 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 369
seangshim 38:d3b7fda44f72 370 motorForwardL(); //直進
seangshim 38:d3b7fda44f72 371 motorForwardR();
seangshim 16:44c763c32b0d 372
seangshim 16:44c763c32b0d 373
seangshim 12:2f0c841e6078 374 int t=0;
seangshim 36:c2b4071970de 375 for(t=0;t<100;t++)
seangshim 12:2f0c841e6078 376 {
seangshim 12:2f0c841e6078 377 printf("%d\r\n", test.read());
seangshim 12:2f0c841e6078 378 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 379 if (test.read() == 1 and flag == 0)
seangshim 1:10af8aaa5b40 380 {
seangshim 1:10af8aaa5b40 381 flag = 1;
seangshim 1:10af8aaa5b40 382 }
seangshim 1:10af8aaa5b40 383 else if (test.read() == 0 and flag == 1)
seangshim 1:10af8aaa5b40 384 {
seangshim 1:10af8aaa5b40 385 flag=0;
seangshim 40:d0b0d2a26cac 386 rightrun += 113;
seangshim 1:10af8aaa5b40 387 }
seangshim 2:37d831f82840 388 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 389 flag2 = 1;
seangshim 2:37d831f82840 390 }
seangshim 2:37d831f82840 391 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 392 flag2 = 0;
seangshim 40:d0b0d2a26cac 393 leftrun += 113;
seangshim 2:37d831f82840 394 }
seangshim 39:a0c63ae63c99 395 printf("%f", leftrun);
seangshim 35:40df2f91cea9 396 printf("\t%f\r\n", rightrun);
seangshim 16:44c763c32b0d 397
seangshim 16:44c763c32b0d 398
seangshim 16:44c763c32b0d 399
seangshim 16:44c763c32b0d 400 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
seangshim 40:d0b0d2a26cac 401 mpu.readGyroData(gyro);
seangshim 40:d0b0d2a26cac 402 int x = accel[0];
seangshim 40:d0b0d2a26cac 403 int y = accel[1];
seangshim 40:d0b0d2a26cac 404 int z = accel[2];
seangshim 40:d0b0d2a26cac 405 int gx = gyro[0];
seangshim 40:d0b0d2a26cac 406 int gy = gyro[1];
seangshim 40:d0b0d2a26cac 407 int gz = gyro[2];
seangshim 40:d0b0d2a26cac 408 fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf, %5d, %5d, %5d\r\n", x, y, z, leftrun, rightrun,gx, gy ,gz);//加速度とフォトインタラプタによる距離を出力
seangshim 17:9bc130ebb5ed 409
seangshim 40:d0b0d2a26cac 410 if (run >= 300000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 27:b4922048ab11 411 break; //つまりゴールについたらこのループからぬける
seangshim 27:b4922048ab11 412 }
seangshim 16:44c763c32b0d 413
seangshim 17:9bc130ebb5ed 414 wait(0.01);
seangshim 27:b4922048ab11 415
seangshim 16:44c763c32b0d 416 }
seangshim 16:44c763c32b0d 417
seangshim 16:44c763c32b0d 418
seangshim 16:44c763c32b0d 419
seangshim 3:c1e0db4832b7 420 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 421
seangshim 38:d3b7fda44f72 422 motorStopR();
seangshim 38:d3b7fda44f72 423 motorStopL();
seangshim 3:c1e0db4832b7 424 wait(2);
seangshim 3:c1e0db4832b7 425 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 426
seangshim 38:d3b7fda44f72 427 motorReverseL(); //機体を反時計回りに９０度回転
seangshim 38:d3b7fda44f72 428 motorForwardR();
seangshim 40:d0b0d2a26cac 429 wait(1);
seangshim 0:a01fda36fde8 430 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 431
seangshim 38:d3b7fda44f72 432 motorStopR();
seangshim 38:d3b7fda44f72 433 motorStopL();
seangshim 3:c1e0db4832b7 434 wait(2);
seangshim 3:c1e0db4832b7 435 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 436
seangshim 38:d3b7fda44f72 437 motorForwardL(); //直進
seangshim 38:d3b7fda44f72 438 motorForwardR();
seangshim 1:10af8aaa5b40 439 wait(2);
seangshim 0:a01fda36fde8 440 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 441
seangshim 38:d3b7fda44f72 442 motorStopR();
seangshim 38:d3b7fda44f72 443 motorStopL();
seangshim 3:c1e0db4832b7 444 wait(2);
seangshim 3:c1e0db4832b7 445 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 446
seangshim 38:d3b7fda44f72 447 motorForwardL(); //機体を時計回りに９０度回転
seangshim 38:d3b7fda44f72 448 motorReverseR();
seangshim 40:d0b0d2a26cac 449 wait(1);
seangshim 0:a01fda36fde8 450 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 451
seangshim 38:d3b7fda44f72 452 motorStopR();
seangshim 38:d3b7fda44f72 453 motorStopL();
seangshim 3:c1e0db4832b7 454 wait(2);
seangshim 3:c1e0db4832b7 455 printf("mortor stop\r\n");
394 10:280a25bcc8bb 456
seangshim 40:d0b0d2a26cac 457 heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
394 30:3bcf14e8dd63 458 if(90<heading<267.5){
seangshim 40:d0b0d2a26cac 459 printf("right\r\n");
seangshim 38:d3b7fda44f72 460 motorForwardL();//右回転
seangshim 38:d3b7fda44f72 461 motorReverseR();
seangshim 40:d0b0d2a26cac 462 wx=(270-heading)*0.004448;
seangshim 40:d0b0d2a26cac 463 wait(wx); //角度のずれ*1度回転するのにかかる時間
seangshim 38:d3b7fda44f72 464 motorStopR();
seangshim 38:d3b7fda44f72 465 motorStopL();
394 30:3bcf14e8dd63 466 wait(1);
seangshim 40:d0b0d2a26cac 467 }
seangshim 40:d0b0d2a26cac 468 else if(0<=heading<=90.0){
seangshim 40:d0b0d2a26cac 469 printf("left\r\n");
seangshim 40:d0b0d2a26cac 470 motorForwardL();//左回転
seangshim 40:d0b0d2a26cac 471 motorReverseR();
seangshim 40:d0b0d2a26cac 472 wy=(270-heading)*0.004448;
seangshim 40:d0b0d2a26cac 473 wait(wy);
seangshim 38:d3b7fda44f72 474 motorStopR();
seangshim 38:d3b7fda44f72 475 motorStopL();
394 30:3bcf14e8dd63 476 wait(1);
seangshim 40:d0b0d2a26cac 477 }
seangshim 40:d0b0d2a26cac 478 else if(272.5<heading<360){
seangshim 40:d0b0d2a26cac 479 printf("left\r\n");
seangshim 40:d0b0d2a26cac 480 motorForwardL();//左回転
seangshim 40:d0b0d2a26cac 481 motorReverseR();
seangshim 40:d0b0d2a26cac 482 wz=(360-heading)*0.004448;
seangshim 40:d0b0d2a26cac 483 wait(wz);
seangshim 38:d3b7fda44f72 484 motorStopR();
seangshim 38:d3b7fda44f72 485 motorStopL();
394 30:3bcf14e8dd63 486 wait(1);
seangshim 40:d0b0d2a26cac 487 }
seangshim 40:d0b0d2a26cac 488 else{
394 30:3bcf14e8dd63 489 wait(5);
394 30:3bcf14e8dd63 490 }
394 30:3bcf14e8dd63 491 printf("searchN\r\n"); //機体が北を向く
394 10:280a25bcc8bb 492
seangshim 0:a01fda36fde8 493 }
seangshim 0:a01fda36fde8 494 }
seangshim 38:d3b7fda44f72 495 motorStopR();
seangshim 38:d3b7fda44f72 496 motorStopL();
seangshim 19:8ad7dcfef11f 497
seangshim 19:8ad7dcfef11f 498
seangshim 35:40df2f91cea9 499 fprintf(fp, "last photo\r\n");
seangshim 33:8d3757a0cd93 500 fprintf(fp,"(%lf)\r\n", run);//最後の加速度とフォトインタラプタによる距離を出力
seangshim 35:40df2f91cea9 501 fprintf(fp, "last left.right\r\n");
seangshim 39:a0c63ae63c99 502 fprintf(fp,"(%lf, %lf)\r\n", leftrun, rightrun);
seangshim 19:8ad7dcfef11f 503 fclose(fp);
seangshim 35:40df2f91cea9 504
seangshim 19:8ad7dcfef11f 505
seangshim 0:a01fda36fde8 506 }
seangshim 1:10af8aaa5b40 507
seangshim 1:10af8aaa5b40 508
seangshim 1:10af8aaa5b40 509
seangshim 38:d3b7fda44f72 510 float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
seangshim 1:10af8aaa5b40 511 {
seangshim 1:10af8aaa5b40 512 float c;
seangshim 1:10af8aaa5b40 513 c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
seangshim 1:10af8aaa5b40 514 return c;
seangshim 1:10af8aaa5b40 515 }
seangshim 38:d3b7fda44f72 516 void motorForwardR() {
seangshim 38:d3b7fda44f72 517 motorStopR();
seangshim 35:40df2f91cea9 518 motor1Dir1 = 1;
seangshim 35:40df2f91cea9 519 motor1Dir2 = 0;
seangshim 27:b4922048ab11 520 }
seangshim 27:b4922048ab11 521
seangshim 38:d3b7fda44f72 522 void motorReverseR() {
seangshim 38:d3b7fda44f72 523 motorStopR();
seangshim 35:40df2f91cea9 524 motor1Dir1 = 0;
seangshim 35:40df2f91cea9 525 motor1Dir2 = 1;
seangshim 27:b4922048ab11 526 }
seangshim 27:b4922048ab11 527
seangshim 38:d3b7fda44f72 528 void motorStopR() {
seangshim 35:40df2f91cea9 529 motor1Dir1 = 0;
seangshim 35:40df2f91cea9 530 motor1Dir2 = 0;
seangshim 35:40df2f91cea9 531 }
seangshim 35:40df2f91cea9 532
seangshim 38:d3b7fda44f72 533 void motorForwardL() {
seangshim 38:d3b7fda44f72 534 motorStopR();
seangshim 35:40df2f91cea9 535 motor2Dir1 = 1;
seangshim 35:40df2f91cea9 536 motor2Dir2 = 0;
seangshim 35:40df2f91cea9 537 }
seangshim 35:40df2f91cea9 538
seangshim 38:d3b7fda44f72 539 void motorReverseL() {
seangshim 38:d3b7fda44f72 540 motorStopR();
seangshim 35:40df2f91cea9 541 motor2Dir1 = 0;
seangshim 35:40df2f91cea9 542 motor2Dir2 = 1;
seangshim 35:40df2f91cea9 543 }
seangshim 35:40df2f91cea9 544
seangshim 38:d3b7fda44f72 545 void motorStopL() {
seangshim 35:40df2f91cea9 546 motor2Dir1 = 0;
seangshim 35:40df2f91cea9 547 motor2Dir2 = 0;
seangshim 27:b4922048ab11 548 }
seangshim 1:10af8aaa5b40 549
seangshim 1:10af8aaa5b40 550
seangshim 1:10af8aaa5b40 551
seangshim 1:10af8aaa5b40 552