cansat_B 2019 / Mbed 2 deprecated cansat_main_new_3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@17:9bc130ebb5ed, 2018-11-20 (annotated)

Committer:
seangshim
Date:
Tue Nov 20 06:43:02 2018 +0000
Revision:
17:9bc130ebb5ed
Parent:
16:44c763c32b0d
Child:
18:2a47ed430cfe
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 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
seangshim 17:9bc130ebb5ed 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{
seangshim 17:9bc130ebb5ed 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 }
seangshim 17:9bc130ebb5ed 62 }
seangshim 1:10af8aaa5b40 63
seangshim 0:a01fda36fde8 64 motor1.stop(0);
seangshim 0:a01fda36fde8 65 motor2.stop(0);
seangshim 0:a01fda36fde8 66
seangshim 16:44c763c32b0d 67 printf("GPS start\r\n");
seangshim 3:c1e0db4832b7 68 FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 1:10af8aaa5b40 69 fprintf(fp, "GPS Start\r\n");
seangshim 0:a01fda36fde8 70 int n;
seangshim 1:10af8aaa5b40 71 for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
seangshim 0:a01fda36fde8 72 {
seangshim 9:fb19a93df88f 73 printf("gps for\r\n");
seangshim 0:a01fda36fde8 74 if(gps.getgps()) //現在地取得
seangshim 0:a01fda36fde8 75 fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
seangshim 0:a01fda36fde8 76
seangshim 0:a01fda36fde8 77 else
seangshim 0:a01fda36fde8 78 fprintf(fp,"No data\r\n");//データ取得に失敗した場合
seangshim 0:a01fda36fde8 79
seangshim 0:a01fda36fde8 80 wait(1);
seangshim 1:10af8aaa5b40 81
seangshim 1:10af8aaa5b40 82 printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
seangshim 1:10af8aaa5b40 83
seangshim 0:a01fda36fde8 84 }
seangshim 0:a01fda36fde8 85 fprintf(fp,"GPS finish\r\n");
seangshim 16:44c763c32b0d 86 fclose(fp); // GPSの測定終了
seangshim 9:fb19a93df88f 87
seangshim 9:fb19a93df88f 88
seangshim 6:574d9a6253c7 89
seangshim 9:fb19a93df88f 90
394 10:280a25bcc8bb 91 compass.init(); //地磁気センサー動作
seangshim 16:44c763c32b0d 92
394 10:280a25bcc8bb 93 int i;
394 10:280a25bcc8bb 94 for(i=0;i<20;i++) //地磁気測定
394 10:280a25bcc8bb 95 {
394 10:280a25bcc8bb 96 pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
394 10:280a25bcc8bb 97 wait(0.5);
seangshim 16:44c763c32b0d 98 }
394 10:280a25bcc8bb 99 float mc1,mc2;
394 10:280a25bcc8bb 100 mc1=3.0;
394 10:280a25bcc8bb 101 mc2=3.0;
394 10:280a25bcc8bb 102 //地磁気センサのキャリブレーション
394 10:280a25bcc8bb 103 motor1.speed(mc1); //車体を時計回りに３秒回転
394 10:280a25bcc8bb 104 motor2.speed(-mc2);
394 10:280a25bcc8bb 105 wait(2);
394 10:280a25bcc8bb 106
394 10:280a25bcc8bb 107 motor1.stop(0);
394 10:280a25bcc8bb 108 motor2.stop(0);
394 10:280a25bcc8bb 109 wait(1);
394 10:280a25bcc8bb 110
394 10:280a25bcc8bb 111 motor1.speed(-mc1); //車体を反時計回りに３秒回転
394 10:280a25bcc8bb 112 motor2.speed(mc2);
394 10:280a25bcc8bb 113 wait(2);
394 10:280a25bcc8bb 114
394 10:280a25bcc8bb 115 motor1.stop(0);
394 10:280a25bcc8bb 116 motor2.stop(0);
394 10:280a25bcc8bb 117 wait(1);
394 10:280a25bcc8bb 118 printf("compass carriblation\r\n"); //キャリブレーション終了
394 10:280a25bcc8bb 119
394 10:280a25bcc8bb 120 float mcr1,mcr2,mcl1,mcl2;
394 10:280a25bcc8bb 121 double heading;
394 10:280a25bcc8bb 122 mcr1=heading*10*0.00026;
394 10:280a25bcc8bb 123 mcr2=heading*10*0.00026;
394 10:280a25bcc8bb 124 mcl1=(PI2-heading)*10*0.00026;
394 10:280a25bcc8bb 125 mcl2=(PI2-heading)*10*0.00026;
394 10:280a25bcc8bb 126 compass.init();
394 10:280a25bcc8bb 127 if(2.5<heading<=M_PI){
394 10:280a25bcc8bb 128 motor1.speed(mcr1);
394 10:280a25bcc8bb 129 motor2.speed(-mcr2);
394 10:280a25bcc8bb 130 wait(1);
394 10:280a25bcc8bb 131 }else if(M_PI<heading<357.5){
394 10:280a25bcc8bb 132 motor1.speed(-mcl1);
394 10:280a25bcc8bb 133 motor2.speed(mcl2);
394 10:280a25bcc8bb 134 wait(1);
394 10:280a25bcc8bb 135 }else{
394 10:280a25bcc8bb 136 wait(2);
394 10:280a25bcc8bb 137 }
394 10:280a25bcc8bb 138 printf("searchN\r\n"); //機体が北を向く
394 10:280a25bcc8bb 139
394 10:280a25bcc8bb 140
seangshim 0:a01fda36fde8 141
seangshim 0:a01fda36fde8 142 mu.startUpdates();//start mesuring the distance(超音波センサー)
seangshim 3:c1e0db4832b7 143 int distance;
seangshim 0:a01fda36fde8 144
seangshim 13:b884f5960fbf 145 int flag=0,flag2=0; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
seangshim 1:10af8aaa5b40 146 //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
seangshim 1:10af8aaa5b40 147 //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
seangshim 2:37d831f82840 148 float rightrun; //変数runをフロートで型づけする
seangshim 2:37d831f82840 149 float leftrun2;
seangshim 4:8b52fd631b32 150 rightrun=0.0;
seangshim 4:8b52fd631b32 151 leftrun2=0.0;
seangshim 9:fb19a93df88f 152
seangshim 17:9bc130ebb5ed 153 float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
seangshim 15:75f014c4c8b8 154 float lowpassValue = 0;
seangshim 15:75f014c4c8b8 155 float highpassValue = 0;
seangshim 15:75f014c4c8b8 156 float speed = 0;//加速度時から算出した速度
seangshim 15:75f014c4c8b8 157 float oldSpeed = 0;//ひとつ前の速度
seangshim 15:75f014c4c8b8 158 float oldAccel = 0;//ひとつ前の加速度
seangshim 15:75f014c4c8b8 159 float difference=0;//変位
seangshim 17:9bc130ebb5ed 160 float timespan=0.01;//時間差
seangshim 17:9bc130ebb5ed 161 float accel[3];//accelを3つの配列で定義。
seangshim 1:10af8aaa5b40 162
seangshim 3:c1e0db4832b7 163 while(1)
seangshim 3:c1e0db4832b7 164 {
seangshim 3:c1e0db4832b7 165 distance=mu.getCurrentDistance();
seangshim 3:c1e0db4832b7 166 printf("%d\r\n",distance);
seangshim 8:d41f5d7d2aa5 167
seangshim 8:d41f5d7d2aa5 168 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
seangshim 15:75f014c4c8b8 169 float x = accel[0]/16384-0.043;//x軸方向の加速度
seangshim 17:9bc130ebb5ed 170 float y = accel[0]/16384;//y軸方向の加速度
seangshim 17:9bc130ebb5ed 171 float z = accel[0]/16384;//z軸方向の加速度
seangshim 15:75f014c4c8b8 172 // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
seangshim 15:75f014c4c8b8 173 lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
seangshim 17:9bc130ebb5ed 174 // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
seangshim 15:75f014c4c8b8 175 highpassValue = x - lowpassValue;
seangshim 17:9bc130ebb5ed 176
seangshim 15:75f014c4c8b8 177 // 速度計算(加速度を台形積分する)
seangshim 15:75f014c4c8b8 178 speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
seangshim 15:75f014c4c8b8 179 oldAccel = highpassValue;
seangshim 17:9bc130ebb5ed 180
seangshim 15:75f014c4c8b8 181 // 変位計算(速度を台形積分する)
seangshim 15:75f014c4c8b8 182 difference = ((speed + oldSpeed) * timespan) / 2 + difference;
seangshim 15:75f014c4c8b8 183 oldSpeed = speed;
seangshim 17:9bc130ebb5ed 184
seangshim 17:9bc130ebb5ed 185 //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
seangshim 17:9bc130ebb5ed 186 //printf("%f,",speed);//速度を表示
seangshim 17:9bc130ebb5ed 187 printf("%f,",difference);//変位を表示
seangshim 3:c1e0db4832b7 188
seangshim 8:d41f5d7d2aa5 189 printf("%d\r\n", test.read()); //フォトインタラプタ
seangshim 3:c1e0db4832b7 190 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 191 if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
seangshim 1:10af8aaa5b40 192 flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
seangshim 3:c1e0db4832b7 193 printf("test.read if\r\n");
seangshim 1:10af8aaa5b40 194 }
seangshim 1:10af8aaa5b40 195 else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
seangshim 1:10af8aaa5b40 196 flag = 0; //まずこれでスイッチを0にして入れる。
seangshim 1:10af8aaa5b40 197 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
seangshim 15:75f014c4c8b8 198 rightrun += 54.95; //総距離runに52.5を加算する
seangshim 3:c1e0db4832b7 199 printf("test.read else\r\n");
seangshim 2:37d831f82840 200 }
seangshim 2:37d831f82840 201 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 202 flag2 = 1;
seangshim 3:c1e0db4832b7 203 printf("test2.read if\r\n");
seangshim 1:10af8aaa5b40 204 }
seangshim 2:37d831f82840 205 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 206 flag2 = 0;
seangshim 15:75f014c4c8b8 207 leftrun2 += 54.95;
seangshim 3:c1e0db4832b7 208 printf("test2.read else\r\n");
seangshim 2:37d831f82840 209 }
seangshim 2:37d831f82840 210 printf("%f", rightrun);
seangshim 2:37d831f82840 211 printf("\t%f\r\n", leftrun2);
seangshim 15:75f014c4c8b8 212 if (rightrun >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 1:10af8aaa5b40 213 break; //つまりゴールについたらこのループからぬける
seangshim 3:c1e0db4832b7 214 }
seangshim 0:a01fda36fde8 215
seangshim 1:10af8aaa5b40 216 motor1.speed(0.5); //通常走行
seangshim 0:a01fda36fde8 217 motor2.speed(0.5);
seangshim 1:10af8aaa5b40 218 //Do something else here
seangshim 1:10af8aaa5b40 219 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
seangshim 1:10af8aaa5b40 220 //the class checks if dist needs to be called.
seangshim 0:a01fda36fde8 221
seangshim 9:fb19a93df88f 222 wait(0.01);
seangshim 0:a01fda36fde8 223
seangshim 3:c1e0db4832b7 224 if(100 < distance && distance < 500) //障害物発見❕
seangshim 0:a01fda36fde8 225 {
seangshim 0:a01fda36fde8 226
seangshim 0:a01fda36fde8 227 printf("if success!\r\n");
seangshim 0:a01fda36fde8 228
seangshim 0:a01fda36fde8 229 float ms1,ms2,msj1,msj2;
seangshim 1:10af8aaa5b40 230 ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
seangshim 1:10af8aaa5b40 231 ms2=1.0;
seangshim 0:a01fda36fde8 232
seangshim 12:2f0c841e6078 233 msj1=1.0; //回転の時
seangshim 12:2f0c841e6078 234 msj2=1.0;
seangshim 0:a01fda36fde8 235
seangshim 0:a01fda36fde8 236 motor1.stop(0);
seangshim 0:a01fda36fde8 237 motor2.stop(0);
seangshim 1:10af8aaa5b40 238 wait(2);
seangshim 0:a01fda36fde8 239 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 240
seangshim 0:a01fda36fde8 241 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 242 motor2.speed(-msj2);
seangshim 12:2f0c841e6078 243 wait(0.77);
seangshim 0:a01fda36fde8 244 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 245
seangshim 3:c1e0db4832b7 246 motor1.stop(0);
seangshim 3:c1e0db4832b7 247 motor2.stop(0);
seangshim 3:c1e0db4832b7 248 wait(2);
seangshim 3:c1e0db4832b7 249 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 250
seangshim 0:a01fda36fde8 251 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 252 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 253 wait(2);
seangshim 0:a01fda36fde8 254 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 255
seangshim 3:c1e0db4832b7 256 motor1.stop(0);
seangshim 3:c1e0db4832b7 257 motor2.stop(0);
seangshim 3:c1e0db4832b7 258 wait(2);
seangshim 3:c1e0db4832b7 259 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 260
seangshim 0:a01fda36fde8 261 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 262 motor2.speed(msj2);
seangshim 12:2f0c841e6078 263 wait(0.77);
seangshim 0:a01fda36fde8 264 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 265
seangshim 3:c1e0db4832b7 266 motor1.stop(0);
seangshim 3:c1e0db4832b7 267 motor2.stop(0);
seangshim 3:c1e0db4832b7 268 wait(2);
seangshim 3:c1e0db4832b7 269 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 270
seangshim 0:a01fda36fde8 271 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 272 motor2.speed(ms2);
seangshim 16:44c763c32b0d 273
seangshim 16:44c763c32b0d 274
seangshim 12:2f0c841e6078 275 int t=0;
seangshim 13:b884f5960fbf 276 for(t=0;t<100;t++)
seangshim 12:2f0c841e6078 277 {
seangshim 12:2f0c841e6078 278 printf("%d\r\n", test.read());
seangshim 12:2f0c841e6078 279 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 280 if (test.read() == 1 and flag == 0)
seangshim 1:10af8aaa5b40 281 {
seangshim 1:10af8aaa5b40 282 flag = 1;
seangshim 1:10af8aaa5b40 283 }
seangshim 1:10af8aaa5b40 284 else if (test.read() == 0 and flag == 1)
seangshim 1:10af8aaa5b40 285 {
seangshim 1:10af8aaa5b40 286 flag=0;
seangshim 2:37d831f82840 287 rightrun += 52.5;
seangshim 1:10af8aaa5b40 288 }
seangshim 2:37d831f82840 289 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 290 flag2 = 1;
seangshim 2:37d831f82840 291 }
seangshim 2:37d831f82840 292 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 293 flag2 = 0;
seangshim 2:37d831f82840 294 leftrun2 += 52.5;
seangshim 2:37d831f82840 295 }
seangshim 2:37d831f82840 296 printf("%f", rightrun);
seangshim 12:2f0c841e6078 297 printf("\t%f\r\n", leftrun2);
seangshim 16:44c763c32b0d 298
seangshim 16:44c763c32b0d 299
seangshim 16:44c763c32b0d 300
seangshim 16:44c763c32b0d 301 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
seangshim 16:44c763c32b0d 302 float x = accel[0]/16384-0.043;//x軸方向の加速度
seangshim 17:9bc130ebb5ed 303 float y = accel[0]/16384;//y軸方向の加速度
seangshim 17:9bc130ebb5ed 304 float z = accel[0]/16384;//z軸方向の加速度
seangshim 16:44c763c32b0d 305 // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
seangshim 16:44c763c32b0d 306 lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
seangshim 17:9bc130ebb5ed 307 // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
seangshim 16:44c763c32b0d 308 highpassValue = x - lowpassValue;
seangshim 17:9bc130ebb5ed 309
seangshim 16:44c763c32b0d 310 // 速度計算(加速度を台形積分する)
seangshim 16:44c763c32b0d 311 speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
seangshim 16:44c763c32b0d 312 oldAccel = highpassValue;
seangshim 17:9bc130ebb5ed 313
seangshim 16:44c763c32b0d 314 // 変位計算(速度を台形積分する)
seangshim 16:44c763c32b0d 315 difference = ((speed + oldSpeed) * timespan) / 2 + difference;
seangshim 16:44c763c32b0d 316 oldSpeed = speed;
seangshim 17:9bc130ebb5ed 317
seangshim 17:9bc130ebb5ed 318 //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
seangshim 17:9bc130ebb5ed 319 //printf("%f,",speed);//速度を表示
seangshim 17:9bc130ebb5ed 320 printf("%f,",difference);//変位を表示
seangshim 16:44c763c32b0d 321
seangshim 17:9bc130ebb5ed 322 wait(0.01);
seangshim 16:44c763c32b0d 323 }
seangshim 16:44c763c32b0d 324
seangshim 16:44c763c32b0d 325
seangshim 16:44c763c32b0d 326
seangshim 3:c1e0db4832b7 327 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 328
seangshim 3:c1e0db4832b7 329 motor1.stop(0);
seangshim 3:c1e0db4832b7 330 motor2.stop(0);
seangshim 3:c1e0db4832b7 331 wait(2);
seangshim 3:c1e0db4832b7 332 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 333
seangshim 0:a01fda36fde8 334 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 335 motor2.speed(msj2);
seangshim 15:75f014c4c8b8 336 wait(1);
seangshim 0:a01fda36fde8 337 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 338
seangshim 3:c1e0db4832b7 339 motor1.stop(0);
seangshim 3:c1e0db4832b7 340 motor2.stop(0);
seangshim 3:c1e0db4832b7 341 wait(2);
seangshim 3:c1e0db4832b7 342 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 343
seangshim 0:a01fda36fde8 344 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 345 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 346 wait(2);
seangshim 0:a01fda36fde8 347 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 348
seangshim 3:c1e0db4832b7 349 motor1.stop(0);
seangshim 3:c1e0db4832b7 350 motor2.stop(0);
seangshim 3:c1e0db4832b7 351 wait(2);
seangshim 3:c1e0db4832b7 352 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 353
seangshim 0:a01fda36fde8 354 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 355 motor2.speed(-msj2);
seangshim 15:75f014c4c8b8 356 wait(1);
seangshim 0:a01fda36fde8 357 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 358
seangshim 3:c1e0db4832b7 359 motor1.stop(0);
seangshim 3:c1e0db4832b7 360 motor2.stop(0);
seangshim 3:c1e0db4832b7 361 wait(2);
seangshim 3:c1e0db4832b7 362 printf("mortor stop\r\n");
394 10:280a25bcc8bb 363
394 10:280a25bcc8bb 364
seangshim 0:a01fda36fde8 365 }
seangshim 0:a01fda36fde8 366 }
seangshim 4:8b52fd631b32 367 motor1.stop(0);
seangshim 4:8b52fd631b32 368 motor2.stop(0);
seangshim 4:8b52fd631b32 369
seangshim 0:a01fda36fde8 370 }
seangshim 1:10af8aaa5b40 371
seangshim 1:10af8aaa5b40 372
seangshim 1:10af8aaa5b40 373
seangshim 1:10af8aaa5b40 374 float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
seangshim 1:10af8aaa5b40 375 {
seangshim 1:10af8aaa5b40 376 float c;
seangshim 1:10af8aaa5b40 377 c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
seangshim 1:10af8aaa5b40 378 return c;
seangshim 1:10af8aaa5b40 379 }
seangshim 1:10af8aaa5b40 380
seangshim 1:10af8aaa5b40 381
seangshim 1:10af8aaa5b40 382
seangshim 1:10af8aaa5b40 383
seangshim 1:10af8aaa5b40 384