cansat-d_2018 / Mbed 2 deprecated cansat_main_new_3

otamesi

Dependencies:   mbed

main.cpp@36:c2b4071970de, 2018-12-15 (annotated)

Committer:
seangshim
Date:
Sat Dec 15 06:06:01 2018 +0000
Revision:
36:c2b4071970de
Parent:
35:40df2f91cea9
Child:
37:24866a13b959
2018/12/15 15:05

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