cansat-d_2018 / Mbed 2 deprecated cansat_main_new_3

otamesi

Dependencies:   mbed

main.cpp@33:8d3757a0cd93, 2018-12-14 (annotated)

Committer:
seangshim
Date:
Fri Dec 14 05:46:43 2018 +0000
Revision:
33:8d3757a0cd93
Parent:
32:c522d463c4fa
Child:
34:0e8f95f07612
2018/12/14 14:46

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