(多分)全部+フライトピン+新しい加速度
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?
User | Revision | Line number | New 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); //車体を反時計回りに3秒回転 |
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); //機体を時計回りに90度回転 |
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); //機体を反時計回りに90度回転 |
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()); ここの部分は単純に5mを足すことにした方がいいかな? |
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); //機体を反時計回りに90度回転 |
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); //機体を時計回りに90度回転 |
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 |