cansat-d_2018 / Mbed 2 deprecated cansat_main_new

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

Dependencies:   mbed

main.cpp@1:10af8aaa5b40, 2018-10-15 (annotated)

Committer:
seangshim
Date:
Mon Oct 15 10:38:06 2018 +0000
Revision:
1:10af8aaa5b40
Parent:
0:a01fda36fde8
Child:
2:37d831f82840
????????GPS??????????????????????????????????????

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 1:10af8aaa5b40 9 DigitalIn test(p15); //ここでピン15からの電圧の値、つまりフォトインタラプタが何か遮るものを検知すればハイの1を返して、
seangshim 1:10af8aaa5b40 10 //何もないつまりスリットの部分ではローの0を返す。それを変数testに代入している
seangshim 1:10af8aaa5b40 11
seangshim 1:10af8aaa5b40 12 Serial pc(USBTX, USBRX); //GPS
seangshim 0:a01fda36fde8 13 GPS gps(p28, p27);
seangshim 0:a01fda36fde8 14
seangshim 1:10af8aaa5b40 15 Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター
seangshim 0:a01fda36fde8 16 Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
seangshim 0:a01fda36fde8 17
seangshim 0:a01fda36fde8 18 void dist(int distance)
seangshim 0:a01fda36fde8 19 {
seangshim 0:a01fda36fde8 20 //put code here to happen when the distance is changed
seangshim 0:a01fda36fde8 21 printf("Distance changed to %dmm\r\n", distance);
seangshim 0:a01fda36fde8 22 }
seangshim 0:a01fda36fde8 23
seangshim 1:10af8aaa5b40 24 ultrasonic mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 超音波センサー
seangshim 0:a01fda36fde8 25 //have updates every .1 seconds and a timeout after 1
seangshim 0:a01fda36fde8 26 //second, and call dist when the distance changes
seangshim 0:a01fda36fde8 27
seangshim 0:a01fda36fde8 28
seangshim 0:a01fda36fde8 29
seangshim 1:10af8aaa5b40 30 LocalFileSystem local("local"); // Create the local filesystem under the name "local"　　 データ保存
seangshim 0:a01fda36fde8 31
seangshim 0:a01fda36fde8 32 int main() {
seangshim 0:a01fda36fde8 33
seangshim 1:10af8aaa5b40 34
seangshim 1:10af8aaa5b40 35 FET = 0; //FET、ニクロム線切断
seangshim 1:10af8aaa5b40 36 wait(60);
seangshim 1:10af8aaa5b40 37 FET = 1;
seangshim 1:10af8aaa5b40 38 wait(30);
seangshim 1:10af8aaa5b40 39 FET = 0;
seangshim 1:10af8aaa5b40 40
seangshim 0:a01fda36fde8 41 motor1.stop(0);
seangshim 0:a01fda36fde8 42 motor2.stop(0);
seangshim 0:a01fda36fde8 43
seangshim 0:a01fda36fde8 44
seangshim 0:a01fda36fde8 45 FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 1:10af8aaa5b40 46 fprintf(fp, "GPS Start\r\n");
seangshim 0:a01fda36fde8 47 int n;
seangshim 1:10af8aaa5b40 48 for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
seangshim 0:a01fda36fde8 49 {
seangshim 0:a01fda36fde8 50
seangshim 0:a01fda36fde8 51 if(gps.getgps()) //現在地取得
seangshim 0:a01fda36fde8 52 fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
seangshim 0:a01fda36fde8 53
seangshim 0:a01fda36fde8 54 else
seangshim 0:a01fda36fde8 55 fprintf(fp,"No data\r\n");//データ取得に失敗した場合
seangshim 0:a01fda36fde8 56
seangshim 0:a01fda36fde8 57 wait(1);
seangshim 1:10af8aaa5b40 58
seangshim 1:10af8aaa5b40 59 printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
seangshim 1:10af8aaa5b40 60
seangshim 0:a01fda36fde8 61 }
seangshim 0:a01fda36fde8 62 fprintf(fp,"GPS finish\r\n");
seangshim 1:10af8aaa5b40 63 fclose(fp); //GPSの測定終了
seangshim 0:a01fda36fde8 64
seangshim 0:a01fda36fde8 65 mu.startUpdates();//start mesuring the distance(超音波センサー)
seangshim 0:a01fda36fde8 66 int distance;
seangshim 0:a01fda36fde8 67
seangshim 0:a01fda36fde8 68 while(1)
seangshim 0:a01fda36fde8 69 {
seangshim 1:10af8aaa5b40 70 int flag; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
seangshim 1:10af8aaa5b40 71 //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
seangshim 1:10af8aaa5b40 72 //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
seangshim 1:10af8aaa5b40 73 float run; //変数runをフロートで型づけする
seangshim 1:10af8aaa5b40 74
seangshim 1:10af8aaa5b40 75 printf("%d\r\n", test.read());
seangshim 1:10af8aaa5b40 76 if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
seangshim 1:10af8aaa5b40 77 flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
seangshim 1:10af8aaa5b40 78 }
seangshim 1:10af8aaa5b40 79 else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
seangshim 1:10af8aaa5b40 80 flag = 0; //まずこれでスイッチを0にして入れる。
seangshim 1:10af8aaa5b40 81 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
seangshim 1:10af8aaa5b40 82 run += 52.5; //総距離runに52.5を加算する
seangshim 1:10af8aaa5b40 83 }
seangshim 1:10af8aaa5b40 84 /* if (run > 250){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 1:10af8aaa5b40 85 break; //つまりゴールについたらこのループからぬける
seangshim 1:10af8aaa5b40 86 }*/
seangshim 0:a01fda36fde8 87
seangshim 1:10af8aaa5b40 88 motor1.speed(0.5); //通常走行
seangshim 0:a01fda36fde8 89 motor2.speed(0.5);
seangshim 1:10af8aaa5b40 90 //Do something else here
seangshim 1:10af8aaa5b40 91 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
seangshim 1:10af8aaa5b40 92 //the class checks if dist needs to be called.
seangshim 0:a01fda36fde8 93
seangshim 0:a01fda36fde8 94 distance=mu.getCurrentDistance();
seangshim 1:10af8aaa5b40 95 wait(0.1);
seangshim 0:a01fda36fde8 96
seangshim 0:a01fda36fde8 97 printf("%d\r\n",distance);
seangshim 0:a01fda36fde8 98
seangshim 1:10af8aaa5b40 99 if(100<distance && distance < 500) //障害物発見❕
seangshim 0:a01fda36fde8 100 {
seangshim 0:a01fda36fde8 101
seangshim 0:a01fda36fde8 102 printf("if success!\r\n");
seangshim 0:a01fda36fde8 103
seangshim 0:a01fda36fde8 104 float ms1,ms2,msj1,msj2;
seangshim 1:10af8aaa5b40 105 ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
seangshim 1:10af8aaa5b40 106 ms2=1.0;
seangshim 0:a01fda36fde8 107
seangshim 1:10af8aaa5b40 108 msj1=1.0; //回転の時
seangshim 1:10af8aaa5b40 109 msj2=1.0;
seangshim 0:a01fda36fde8 110
seangshim 0:a01fda36fde8 111 motor1.stop(0);
seangshim 0:a01fda36fde8 112 motor2.stop(0);
seangshim 1:10af8aaa5b40 113 wait(2);
seangshim 0:a01fda36fde8 114 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 115
seangshim 0:a01fda36fde8 116 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 117 motor2.speed(-msj2);
seangshim 1:10af8aaa5b40 118 wait(2);
seangshim 0:a01fda36fde8 119 printf("mortor rotation\r\n");
seangshim 0:a01fda36fde8 120
seangshim 0:a01fda36fde8 121 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 122 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 123 wait(2);
seangshim 0:a01fda36fde8 124 printf("mortor straight\r\n");
seangshim 0:a01fda36fde8 125
seangshim 0:a01fda36fde8 126 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 127 motor2.speed(msj2);
seangshim 1:10af8aaa5b40 128 wait(2);
seangshim 0:a01fda36fde8 129 printf("mortor rotation\r\n");
seangshim 0:a01fda36fde8 130
seangshim 0:a01fda36fde8 131 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 132 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 133
seangshim 1:10af8aaa5b40 134 printf("%d\r\n", test.read());
seangshim 1:10af8aaa5b40 135 if (test.read() == 1 and flag == 0)
seangshim 1:10af8aaa5b40 136 {
seangshim 1:10af8aaa5b40 137 flag = 1;
seangshim 1:10af8aaa5b40 138 }
seangshim 1:10af8aaa5b40 139 else if (test.read() == 0 and flag == 1)
seangshim 1:10af8aaa5b40 140 {
seangshim 1:10af8aaa5b40 141 flag=0;
seangshim 1:10af8aaa5b40 142 }
seangshim 1:10af8aaa5b40 143
seangshim 1:10af8aaa5b40 144 wait(2);
seangshim 0:a01fda36fde8 145 printf("mortor straight\r\n");
seangshim 0:a01fda36fde8 146
seangshim 0:a01fda36fde8 147 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 148 motor2.speed(msj2);
seangshim 1:10af8aaa5b40 149 wait(2);
seangshim 0:a01fda36fde8 150 printf("mortor rotation\r\n");
seangshim 0:a01fda36fde8 151
seangshim 0:a01fda36fde8 152 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 153 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 154 wait(2);
seangshim 0:a01fda36fde8 155 printf("mortor straight\r\n");
seangshim 0:a01fda36fde8 156
seangshim 0:a01fda36fde8 157 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 158 motor2.speed(-msj2);
seangshim 1:10af8aaa5b40 159 wait(2);
seangshim 0:a01fda36fde8 160 printf("mortor rotation\r\n");
seangshim 0:a01fda36fde8 161 }
seangshim 0:a01fda36fde8 162 }
seangshim 0:a01fda36fde8 163 }
seangshim 1:10af8aaa5b40 164
seangshim 1:10af8aaa5b40 165
seangshim 1:10af8aaa5b40 166
seangshim 1:10af8aaa5b40 167 float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
seangshim 1:10af8aaa5b40 168 {
seangshim 1:10af8aaa5b40 169 float c;
seangshim 1:10af8aaa5b40 170 c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
seangshim 1:10af8aaa5b40 171 return c;
seangshim 1:10af8aaa5b40 172 }
seangshim 1:10af8aaa5b40 173
seangshim 1:10af8aaa5b40 174
seangshim 1:10af8aaa5b40 175
seangshim 1:10af8aaa5b40 176
seangshim 1:10af8aaa5b40 177