cansat-d_2018 / Mbed 2 deprecated cansat_main_new

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

Dependencies:   mbed

main.cpp@7:1c1b782263cf, 2018-10-22 (annotated)

Committer:
394
Date:
Mon Oct 22 01:28:54 2018 +0000
Revision:
7:1c1b782263cf
Parent:
4:8b52fd631b32 
10/22; 10:29

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 0:a01fda36fde8 5
seangshim 1:10af8aaa5b40 6 DigitalOut FET(p21); //FET
seangshim 1:10af8aaa5b40 7
seangshim 1:10af8aaa5b40 8 InterruptIn button1(p15); //フォトインタラプタ
seangshim 2:37d831f82840 9 InterruptIn button2(p18);
seangshim 1:10af8aaa5b40 10 DigitalIn test(p15); //ここでピン15からの電圧の値、つまりフォトインタラプタが何か遮るものを検知すればハイの1を返して、
seangshim 1:10af8aaa5b40 11 //何もないつまりスリットの部分ではローの0を返す。それを変数testに代入している
seangshim 2:37d831f82840 12 DigitalIn test2(p18);
seangshim 1:10af8aaa5b40 13
seangshim 1:10af8aaa5b40 14 Serial pc(USBTX, USBRX); //GPS
seangshim 0:a01fda36fde8 15 GPS gps(p28, p27);
seangshim 0:a01fda36fde8 16
seangshim 1:10af8aaa5b40 17 Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター
seangshim 0:a01fda36fde8 18 Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
seangshim 0:a01fda36fde8 19
seangshim 0:a01fda36fde8 20 void dist(int distance)
seangshim 0:a01fda36fde8 21 {
seangshim 0:a01fda36fde8 22 //put code here to happen when the distance is changed
seangshim 0:a01fda36fde8 23 printf("Distance changed to %dmm\r\n", distance);
seangshim 0:a01fda36fde8 24 }
seangshim 0:a01fda36fde8 25
seangshim 1:10af8aaa5b40 26 ultrasonic mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 超音波センサー
seangshim 0:a01fda36fde8 27 //have updates every .1 seconds and a timeout after 1
seangshim 0:a01fda36fde8 28 //second, and call dist when the distance changes
seangshim 0:a01fda36fde8 29
seangshim 0:a01fda36fde8 30
seangshim 0:a01fda36fde8 31
seangshim 1:10af8aaa5b40 32 LocalFileSystem local("local"); // Create the local filesystem under the name "local"　　 データ保存
seangshim 0:a01fda36fde8 33
seangshim 0:a01fda36fde8 34 int main() {
seangshim 0:a01fda36fde8 35
seangshim 1:10af8aaa5b40 36
seangshim 3:c1e0db4832b7 37 /* FET = 0; //FET、ニクロム線切断
seangshim 1:10af8aaa5b40 38 wait(60);
seangshim 1:10af8aaa5b40 39 FET = 1;
seangshim 1:10af8aaa5b40 40 wait(30);
seangshim 3:c1e0db4832b7 41 FET = 0; */
seangshim 1:10af8aaa5b40 42
seangshim 0:a01fda36fde8 43 motor1.stop(0);
seangshim 0:a01fda36fde8 44 motor2.stop(0);
seangshim 0:a01fda36fde8 45
seangshim 0:a01fda36fde8 46
seangshim 3:c1e0db4832b7 47 FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 1:10af8aaa5b40 48 fprintf(fp, "GPS Start\r\n");
seangshim 0:a01fda36fde8 49 int n;
seangshim 1:10af8aaa5b40 50 for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
seangshim 0:a01fda36fde8 51 {
seangshim 0:a01fda36fde8 52
seangshim 0:a01fda36fde8 53 if(gps.getgps()) //現在地取得
seangshim 0:a01fda36fde8 54 fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
seangshim 0:a01fda36fde8 55
seangshim 0:a01fda36fde8 56 else
seangshim 0:a01fda36fde8 57 fprintf(fp,"No data\r\n");//データ取得に失敗した場合
seangshim 0:a01fda36fde8 58
seangshim 0:a01fda36fde8 59 wait(1);
seangshim 1:10af8aaa5b40 60
seangshim 1:10af8aaa5b40 61 printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
seangshim 1:10af8aaa5b40 62
seangshim 0:a01fda36fde8 63 }
seangshim 0:a01fda36fde8 64 fprintf(fp,"GPS finish\r\n");
seangshim 1:10af8aaa5b40 65 fclose(fp); //GPSの測定終了
394 7:1c1b782263cf 66
394 7:1c1b782263cf 67 float mc1,mc2; //地磁気センサのキャリブレーション
394 7:1c1b782263cf 68 mc1=3.0;
394 7:1c1b782263cf 69 mc2=3.0;
394 7:1c1b782263cf 70
394 7:1c1b782263cf 71 motor1.speed(mc1); //車体を時計回りに3秒回転
394 7:1c1b782263cf 72 motor2.speed(-mc2);
394 7:1c1b782263cf 73 wait(2);
394 7:1c1b782263cf 74
394 7:1c1b782263cf 75 motor1.stop(0);
394 7:1c1b782263cf 76 motor2.stop(0);
394 7:1c1b782263cf 77 wait(1);
394 7:1c1b782263cf 78
394 7:1c1b782263cf 79 motor1.speed(-mc1); //車体を反時計回りに３秒回転
394 7:1c1b782263cf 80 motor2.speed(mc2);
394 7:1c1b782263cf 81 wait(2);
394 7:1c1b782263cf 82
394 7:1c1b782263cf 83 motor1.stop(0);
394 7:1c1b782263cf 84 motor2.stop(0);
394 7:1c1b782263cf 85 wait(1);
394 7:1c1b782263cf 86 printf("compass carriblation\r\n"); //キャリブレーション終了
394 7:1c1b782263cf 87
394 7:1c1b782263cf 88 if(M_PI<raw<357.5){
394 7:1c1b782263cf 89 motor1.speed(mc1);
394 7:1c1b782263cf 90 motor2.speed(-mc2);
394 7:1c1b782263cf 91 wait(1);
394 7:1c1b782263cf 92 }else if(357.5<=raw<PI2 && raw<=2.5){
394 7:1c1b782263cf 93 motor1.speed(-mc1);
394 7:1c1b782263cf 94 motor2.speed(mc2);
394 7:1c1b782263cf 95 wait(1);
394 7:1c1b782263cf 96 }else
394 7:1c1b782263cf 97 motor1.stop(0);
394 7:1c1b782263cf 98 motor2.stop(0);
394 7:1c1b782263cf 99 wait(2);
394 7:1c1b782263cf 100 printf("search north\r\n"); //機体が北を向く
394 7:1c1b782263cf 101
seangshim 0:a01fda36fde8 102 mu.startUpdates();//start mesuring the distance(超音波センサー)
seangshim 3:c1e0db4832b7 103 int distance;
seangshim 0:a01fda36fde8 104
seangshim 2:37d831f82840 105 int flag,flag2; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
seangshim 1:10af8aaa5b40 106 //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
seangshim 1:10af8aaa5b40 107 //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
seangshim 2:37d831f82840 108 float rightrun; //変数runをフロートで型づけする
seangshim 2:37d831f82840 109 float leftrun2;
seangshim 4:8b52fd631b32 110 rightrun=0.0;
seangshim 4:8b52fd631b32 111 leftrun2=0.0;
seangshim 1:10af8aaa5b40 112
seangshim 3:c1e0db4832b7 113 while(1)
seangshim 3:c1e0db4832b7 114 {
seangshim 3:c1e0db4832b7 115 distance=mu.getCurrentDistance();
seangshim 3:c1e0db4832b7 116 printf("%d\r\n",distance);
seangshim 3:c1e0db4832b7 117
seangshim 3:c1e0db4832b7 118
seangshim 1:10af8aaa5b40 119 printf("%d\r\n", test.read());
seangshim 3:c1e0db4832b7 120 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 121 if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
seangshim 1:10af8aaa5b40 122 flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
seangshim 3:c1e0db4832b7 123 printf("test.read if\r\n");
seangshim 1:10af8aaa5b40 124 }
seangshim 1:10af8aaa5b40 125 else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
seangshim 1:10af8aaa5b40 126 flag = 0; //まずこれでスイッチを0にして入れる。
seangshim 1:10af8aaa5b40 127 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
seangshim 2:37d831f82840 128 rightrun += 52.5; //総距離runに52.5を加算する
seangshim 3:c1e0db4832b7 129 printf("test.read else\r\n");
seangshim 2:37d831f82840 130 }
seangshim 2:37d831f82840 131 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 132 flag2 = 1;
seangshim 3:c1e0db4832b7 133 printf("test2.read if\r\n");
seangshim 1:10af8aaa5b40 134 }
seangshim 2:37d831f82840 135 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 136 flag2 = 0;
seangshim 2:37d831f82840 137 leftrun2 += 52.5;
seangshim 3:c1e0db4832b7 138 printf("test2.read else\r\n");
seangshim 2:37d831f82840 139 }
seangshim 2:37d831f82840 140 printf("%f", rightrun);
seangshim 2:37d831f82840 141 printf("\t%f\r\n", leftrun2);
seangshim 3:c1e0db4832b7 142 if (rightrun > 250){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 1:10af8aaa5b40 143 break; //つまりゴールについたらこのループからぬける
seangshim 3:c1e0db4832b7 144 }
seangshim 0:a01fda36fde8 145
seangshim 1:10af8aaa5b40 146 motor1.speed(0.5); //通常走行
seangshim 0:a01fda36fde8 147 motor2.speed(0.5);
seangshim 1:10af8aaa5b40 148 //Do something else here
seangshim 1:10af8aaa5b40 149 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
seangshim 1:10af8aaa5b40 150 //the class checks if dist needs to be called.
seangshim 0:a01fda36fde8 151
seangshim 3:c1e0db4832b7 152 wait(0.2);
seangshim 0:a01fda36fde8 153
seangshim 3:c1e0db4832b7 154 if(100 < distance && distance < 500) //障害物発見❕
seangshim 0:a01fda36fde8 155 {
seangshim 0:a01fda36fde8 156
seangshim 0:a01fda36fde8 157 printf("if success!\r\n");
seangshim 0:a01fda36fde8 158
seangshim 0:a01fda36fde8 159 float ms1,ms2,msj1,msj2;
seangshim 1:10af8aaa5b40 160 ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
seangshim 1:10af8aaa5b40 161 ms2=1.0;
seangshim 0:a01fda36fde8 162
seangshim 1:10af8aaa5b40 163 msj1=1.0; //回転の時
seangshim 1:10af8aaa5b40 164 msj2=1.0;
seangshim 0:a01fda36fde8 165
seangshim 0:a01fda36fde8 166 motor1.stop(0);
seangshim 0:a01fda36fde8 167 motor2.stop(0);
seangshim 1:10af8aaa5b40 168 wait(2);
seangshim 0:a01fda36fde8 169 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 170
seangshim 0:a01fda36fde8 171 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 172 motor2.speed(-msj2);
seangshim 1:10af8aaa5b40 173 wait(2);
seangshim 0:a01fda36fde8 174 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 175
seangshim 3:c1e0db4832b7 176 motor1.stop(0);
seangshim 3:c1e0db4832b7 177 motor2.stop(0);
seangshim 3:c1e0db4832b7 178 wait(2);
seangshim 3:c1e0db4832b7 179 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 180
seangshim 0:a01fda36fde8 181 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 182 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 183 wait(2);
seangshim 0:a01fda36fde8 184 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 185
seangshim 3:c1e0db4832b7 186 motor1.stop(0);
seangshim 3:c1e0db4832b7 187 motor2.stop(0);
seangshim 3:c1e0db4832b7 188 wait(2);
seangshim 3:c1e0db4832b7 189 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 190
seangshim 0:a01fda36fde8 191 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 192 motor2.speed(msj2);
seangshim 1:10af8aaa5b40 193 wait(2);
seangshim 0:a01fda36fde8 194 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 195
seangshim 3:c1e0db4832b7 196 motor1.stop(0);
seangshim 3:c1e0db4832b7 197 motor2.stop(0);
seangshim 3:c1e0db4832b7 198 wait(2);
seangshim 3:c1e0db4832b7 199 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 200
seangshim 0:a01fda36fde8 201 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 202 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 203
seangshim 2:37d831f82840 204 /* printf("%d\r\n", test.read()); ここの部分は単純に５mを足すことにした方がいいかな？
seangshim 1:10af8aaa5b40 205 if (test.read() == 1 and flag == 0)
seangshim 1:10af8aaa5b40 206 {
seangshim 1:10af8aaa5b40 207 flag = 1;
seangshim 1:10af8aaa5b40 208 }
seangshim 1:10af8aaa5b40 209 else if (test.read() == 0 and flag == 1)
seangshim 1:10af8aaa5b40 210 {
seangshim 1:10af8aaa5b40 211 flag=0;
seangshim 2:37d831f82840 212 rightrun += 52.5;
seangshim 1:10af8aaa5b40 213 }
seangshim 2:37d831f82840 214 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 215 flag2 = 1;
seangshim 2:37d831f82840 216 }
seangshim 2:37d831f82840 217 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 218 flag2 = 0;
seangshim 2:37d831f82840 219 leftrun2 += 52.5;
seangshim 2:37d831f82840 220 }
seangshim 2:37d831f82840 221 printf("%f", rightrun);
seangshim 2:37d831f82840 222 printf("\t%f\r\n", leftrun2);*/
seangshim 1:10af8aaa5b40 223 wait(2);
seangshim 3:c1e0db4832b7 224 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 225
seangshim 3:c1e0db4832b7 226 motor1.stop(0);
seangshim 3:c1e0db4832b7 227 motor2.stop(0);
seangshim 3:c1e0db4832b7 228 wait(2);
seangshim 3:c1e0db4832b7 229 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 230
seangshim 0:a01fda36fde8 231 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 232 motor2.speed(msj2);
seangshim 1:10af8aaa5b40 233 wait(2);
seangshim 0:a01fda36fde8 234 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 235
seangshim 3:c1e0db4832b7 236 motor1.stop(0);
seangshim 3:c1e0db4832b7 237 motor2.stop(0);
seangshim 3:c1e0db4832b7 238 wait(2);
seangshim 3:c1e0db4832b7 239 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 240
seangshim 0:a01fda36fde8 241 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 242 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 243 wait(2);
seangshim 0:a01fda36fde8 244 printf("mortor straight\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(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 252 motor2.speed(-msj2);
seangshim 1:10af8aaa5b40 253 wait(2);
seangshim 0:a01fda36fde8 254 printf("mortor rotation\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 }
seangshim 4:8b52fd631b32 262
seangshim 4:8b52fd631b32 263 motor1.stop(0);
seangshim 4:8b52fd631b32 264 motor2.stop(0);
seangshim 4:8b52fd631b32 265
seangshim 0:a01fda36fde8 266 }
seangshim 1:10af8aaa5b40 267
seangshim 1:10af8aaa5b40 268
seangshim 1:10af8aaa5b40 269
seangshim 1:10af8aaa5b40 270 float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
seangshim 1:10af8aaa5b40 271 {
seangshim 1:10af8aaa5b40 272 float c;
seangshim 1:10af8aaa5b40 273 c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
seangshim 1:10af8aaa5b40 274 return c;
seangshim 1:10af8aaa5b40 275 }
seangshim 1:10af8aaa5b40 276
seangshim 1:10af8aaa5b40 277
seangshim 1:10af8aaa5b40 278
seangshim 1:10af8aaa5b40 279
seangshim 1:10af8aaa5b40 280