cansat-d_2018 / Mbed 2 deprecated cansat_main_new

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

Dependencies:   mbed

main.cpp@9:fb19a93df88f, 2018-10-26 (annotated)

Committer:
seangshim
Date:
Fri Oct 26 07:04:55 2018 +0000
Revision:
9:fb19a93df88f
Parent:
8:d41f5d7d2aa5
Child:
10:280a25bcc8bb
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
seangshim 9:fb19a93df88f 13 MPU6050 mpu(p9,p10); //(SDA,SCL)のピンをアサインしてね☆ 加速度センサー
seangshim 9:fb19a93df88f 14
seangshim 0:a01fda36fde8 15
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 1:10af8aaa5b40 44
seangshim 3:c1e0db4832b7 45 /* FET = 0; //FET、ニクロム線切断
seangshim 1:10af8aaa5b40 46 wait(60);
seangshim 1:10af8aaa5b40 47 FET = 1;
seangshim 1:10af8aaa5b40 48 wait(30);
seangshim 3:c1e0db4832b7 49 FET = 0; */
seangshim 1:10af8aaa5b40 50
seangshim 0:a01fda36fde8 51 motor1.stop(0);
seangshim 0:a01fda36fde8 52 motor2.stop(0);
seangshim 0:a01fda36fde8 53
seangshim 9:fb19a93df88f 54 printf("GPS start\r\n");
seangshim 3:c1e0db4832b7 55 FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 1:10af8aaa5b40 56 fprintf(fp, "GPS Start\r\n");
seangshim 0:a01fda36fde8 57 int n;
seangshim 1:10af8aaa5b40 58 for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
seangshim 0:a01fda36fde8 59 {
seangshim 9:fb19a93df88f 60 printf("gps for\r\n");
seangshim 0:a01fda36fde8 61 if(gps.getgps()) //現在地取得
seangshim 0:a01fda36fde8 62 fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
seangshim 0:a01fda36fde8 63
seangshim 0:a01fda36fde8 64 else
seangshim 0:a01fda36fde8 65 fprintf(fp,"No data\r\n");//データ取得に失敗した場合
seangshim 0:a01fda36fde8 66
seangshim 0:a01fda36fde8 67 wait(1);
seangshim 1:10af8aaa5b40 68
seangshim 1:10af8aaa5b40 69 printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
seangshim 1:10af8aaa5b40 70
seangshim 0:a01fda36fde8 71 }
seangshim 0:a01fda36fde8 72 fprintf(fp,"GPS finish\r\n");
seangshim 8:d41f5d7d2aa5 73 fclose(fp); //GPSの測定終了
seangshim 9:fb19a93df88f 74
seangshim 9:fb19a93df88f 75
seangshim 6:574d9a6253c7 76
seangshim 9:fb19a93df88f 77
seangshim 9:fb19a93df88f 78 compass.init(); //地磁気センサー動作
seangshim 9:fb19a93df88f 79
seangshim 9:fb19a93df88f 80 int i;
seangshim 9:fb19a93df88f 81 for(i=0;i<20;i++) //地磁気測定
seangshim 9:fb19a93df88f 82 {
seangshim 9:fb19a93df88f 83 pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
seangshim 9:fb19a93df88f 84 wait(0.5);
seangshim 9:fb19a93df88f 85 }
seangshim 0:a01fda36fde8 86
seangshim 0:a01fda36fde8 87 mu.startUpdates();//start mesuring the distance(超音波センサー)
seangshim 3:c1e0db4832b7 88 int distance;
seangshim 0:a01fda36fde8 89
seangshim 2:37d831f82840 90 int flag,flag2; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
seangshim 1:10af8aaa5b40 91 //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
seangshim 1:10af8aaa5b40 92 //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
seangshim 2:37d831f82840 93 float rightrun; //変数runをフロートで型づけする
seangshim 2:37d831f82840 94 float leftrun2;
seangshim 4:8b52fd631b32 95 rightrun=0.0;
seangshim 4:8b52fd631b32 96 leftrun2=0.0;
seangshim 9:fb19a93df88f 97
seangshim 9:fb19a93df88f 98 float accel[3];//accelを3つの配列で定義。 加速度センサー
seangshim 1:10af8aaa5b40 99
seangshim 3:c1e0db4832b7 100 while(1)
seangshim 3:c1e0db4832b7 101 {
seangshim 3:c1e0db4832b7 102 distance=mu.getCurrentDistance();
seangshim 3:c1e0db4832b7 103 printf("%d\r\n",distance);
seangshim 8:d41f5d7d2aa5 104
seangshim 8:d41f5d7d2aa5 105 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
seangshim 8:d41f5d7d2aa5 106 float x = accel[0]/16384;//x軸方向の加速度
seangshim 8:d41f5d7d2aa5 107 float y = accel[1]/16384;//y軸方向の加速度
seangshim 8:d41f5d7d2aa5 108 float z = accel[2]/16384;//z軸方向の加速度
seangshim 8:d41f5d7d2aa5 109 printf("accel x:%f y:%f z:%f\r\n",x,y,z);//一応表示しとくやで～～
seangshim 3:c1e0db4832b7 110
seangshim 3:c1e0db4832b7 111
seangshim 8:d41f5d7d2aa5 112 printf("%d\r\n", test.read()); //フォトインタラプタ
seangshim 3:c1e0db4832b7 113 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 114 if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
seangshim 1:10af8aaa5b40 115 flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
seangshim 3:c1e0db4832b7 116 printf("test.read if\r\n");
seangshim 1:10af8aaa5b40 117 }
seangshim 1:10af8aaa5b40 118 else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
seangshim 1:10af8aaa5b40 119 flag = 0; //まずこれでスイッチを0にして入れる。
seangshim 1:10af8aaa5b40 120 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
seangshim 2:37d831f82840 121 rightrun += 52.5; //総距離runに52.5を加算する
seangshim 3:c1e0db4832b7 122 printf("test.read else\r\n");
seangshim 2:37d831f82840 123 }
seangshim 2:37d831f82840 124 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 125 flag2 = 1;
seangshim 3:c1e0db4832b7 126 printf("test2.read if\r\n");
seangshim 1:10af8aaa5b40 127 }
seangshim 2:37d831f82840 128 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 129 flag2 = 0;
seangshim 2:37d831f82840 130 leftrun2 += 52.5;
seangshim 3:c1e0db4832b7 131 printf("test2.read else\r\n");
seangshim 2:37d831f82840 132 }
seangshim 2:37d831f82840 133 printf("%f", rightrun);
seangshim 2:37d831f82840 134 printf("\t%f\r\n", leftrun2);
seangshim 3:c1e0db4832b7 135 if (rightrun > 250){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 1:10af8aaa5b40 136 break; //つまりゴールについたらこのループからぬける
seangshim 3:c1e0db4832b7 137 }
seangshim 0:a01fda36fde8 138
seangshim 1:10af8aaa5b40 139 motor1.speed(0.5); //通常走行
seangshim 0:a01fda36fde8 140 motor2.speed(0.5);
seangshim 1:10af8aaa5b40 141 //Do something else here
seangshim 1:10af8aaa5b40 142 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
seangshim 1:10af8aaa5b40 143 //the class checks if dist needs to be called.
seangshim 0:a01fda36fde8 144
seangshim 9:fb19a93df88f 145 wait(0.01);
seangshim 0:a01fda36fde8 146
seangshim 3:c1e0db4832b7 147 if(100 < distance && distance < 500) //障害物発見❕
seangshim 0:a01fda36fde8 148 {
seangshim 0:a01fda36fde8 149
seangshim 0:a01fda36fde8 150 printf("if success!\r\n");
seangshim 0:a01fda36fde8 151
seangshim 0:a01fda36fde8 152 float ms1,ms2,msj1,msj2;
seangshim 1:10af8aaa5b40 153 ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
seangshim 1:10af8aaa5b40 154 ms2=1.0;
seangshim 0:a01fda36fde8 155
seangshim 1:10af8aaa5b40 156 msj1=1.0; //回転の時
seangshim 1:10af8aaa5b40 157 msj2=1.0;
seangshim 0:a01fda36fde8 158
seangshim 0:a01fda36fde8 159 motor1.stop(0);
seangshim 0:a01fda36fde8 160 motor2.stop(0);
seangshim 1:10af8aaa5b40 161 wait(2);
seangshim 0:a01fda36fde8 162 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 163
seangshim 0:a01fda36fde8 164 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 165 motor2.speed(-msj2);
seangshim 1:10af8aaa5b40 166 wait(2);
seangshim 0:a01fda36fde8 167 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 168
seangshim 3:c1e0db4832b7 169 motor1.stop(0);
seangshim 3:c1e0db4832b7 170 motor2.stop(0);
seangshim 3:c1e0db4832b7 171 wait(2);
seangshim 3:c1e0db4832b7 172 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 173
seangshim 0:a01fda36fde8 174 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 175 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 176 wait(2);
seangshim 0:a01fda36fde8 177 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 178
seangshim 3:c1e0db4832b7 179 motor1.stop(0);
seangshim 3:c1e0db4832b7 180 motor2.stop(0);
seangshim 3:c1e0db4832b7 181 wait(2);
seangshim 3:c1e0db4832b7 182 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 183
seangshim 0:a01fda36fde8 184 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 185 motor2.speed(msj2);
seangshim 1:10af8aaa5b40 186 wait(2);
seangshim 0:a01fda36fde8 187 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 188
seangshim 3:c1e0db4832b7 189 motor1.stop(0);
seangshim 3:c1e0db4832b7 190 motor2.stop(0);
seangshim 3:c1e0db4832b7 191 wait(2);
seangshim 3:c1e0db4832b7 192 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 193
seangshim 0:a01fda36fde8 194 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 195 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 196
seangshim 2:37d831f82840 197 /* printf("%d\r\n", test.read()); ここの部分は単純に５mを足すことにした方がいいかな？
seangshim 1:10af8aaa5b40 198 if (test.read() == 1 and flag == 0)
seangshim 1:10af8aaa5b40 199 {
seangshim 1:10af8aaa5b40 200 flag = 1;
seangshim 1:10af8aaa5b40 201 }
seangshim 1:10af8aaa5b40 202 else if (test.read() == 0 and flag == 1)
seangshim 1:10af8aaa5b40 203 {
seangshim 1:10af8aaa5b40 204 flag=0;
seangshim 2:37d831f82840 205 rightrun += 52.5;
seangshim 1:10af8aaa5b40 206 }
seangshim 2:37d831f82840 207 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 208 flag2 = 1;
seangshim 2:37d831f82840 209 }
seangshim 2:37d831f82840 210 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 211 flag2 = 0;
seangshim 2:37d831f82840 212 leftrun2 += 52.5;
seangshim 2:37d831f82840 213 }
seangshim 2:37d831f82840 214 printf("%f", rightrun);
seangshim 2:37d831f82840 215 printf("\t%f\r\n", leftrun2);*/
seangshim 1:10af8aaa5b40 216 wait(2);
seangshim 3:c1e0db4832b7 217 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 218
seangshim 3:c1e0db4832b7 219 motor1.stop(0);
seangshim 3:c1e0db4832b7 220 motor2.stop(0);
seangshim 3:c1e0db4832b7 221 wait(2);
seangshim 3:c1e0db4832b7 222 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 223
seangshim 0:a01fda36fde8 224 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 225 motor2.speed(msj2);
seangshim 1:10af8aaa5b40 226 wait(2);
seangshim 0:a01fda36fde8 227 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 228
seangshim 3:c1e0db4832b7 229 motor1.stop(0);
seangshim 3:c1e0db4832b7 230 motor2.stop(0);
seangshim 3:c1e0db4832b7 231 wait(2);
seangshim 3:c1e0db4832b7 232 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 233
seangshim 0:a01fda36fde8 234 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 235 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 236 wait(2);
seangshim 0:a01fda36fde8 237 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 238
seangshim 3:c1e0db4832b7 239 motor1.stop(0);
seangshim 3:c1e0db4832b7 240 motor2.stop(0);
seangshim 3:c1e0db4832b7 241 wait(2);
seangshim 3:c1e0db4832b7 242 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 243
seangshim 0:a01fda36fde8 244 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 245 motor2.speed(-msj2);
seangshim 1:10af8aaa5b40 246 wait(2);
seangshim 0:a01fda36fde8 247 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 248
seangshim 3:c1e0db4832b7 249 motor1.stop(0);
seangshim 3:c1e0db4832b7 250 motor2.stop(0);
seangshim 3:c1e0db4832b7 251 wait(2);
seangshim 3:c1e0db4832b7 252 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 253 }
seangshim 0:a01fda36fde8 254 }
seangshim 4:8b52fd631b32 255
seangshim 4:8b52fd631b32 256 motor1.stop(0);
seangshim 4:8b52fd631b32 257 motor2.stop(0);
seangshim 4:8b52fd631b32 258
seangshim 0:a01fda36fde8 259 }
seangshim 1:10af8aaa5b40 260
seangshim 1:10af8aaa5b40 261
seangshim 1:10af8aaa5b40 262
seangshim 1:10af8aaa5b40 263 float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
seangshim 1:10af8aaa5b40 264 {
seangshim 1:10af8aaa5b40 265 float c;
seangshim 1:10af8aaa5b40 266 c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
seangshim 1:10af8aaa5b40 267 return c;
seangshim 1:10af8aaa5b40 268 }
seangshim 1:10af8aaa5b40 269
seangshim 1:10af8aaa5b40 270
seangshim 1:10af8aaa5b40 271
seangshim 1:10af8aaa5b40 272
seangshim 1:10af8aaa5b40 273