cansat-d_2018 / Mbed 2 deprecated cansat_main_new

（多分）全部+フライトピン+新しい加速度

Dependencies:   mbed

main.cpp@27:b4922048ab11, 2018-12-11 (annotated)

Committer:
seangshim
Date:
Tue Dec 11 10:23:35 2018 +0000
Revision:
27:b4922048ab11
Parent:
23:2ca79873ef44
Child:
28:5e21ce413558
new

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