ニクロム線切断、GPS取得、モーター、障害物回避、フォトインタラプタ、(距離推定)、加速度と地磁気のデータ取得

Dependencies:   mbed

Fork of cansat_main by cansat-d_2018

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