cansat-d_2018 / Mbed 2 deprecated cansat_main_new

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

Dependencies:   mbed

main.cpp@24:a09b08663a7c, 2018-12-07 (annotated)

Committer:
394
Date:
Fri Dec 07 12:45:05 2018 +0000
Revision:
24:a09b08663a7c
Parent:
22:1e01f4f7097c
Child:
25:6680d67bdf24
12/07; 21:45

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