cansat_B 2019
/
CanSatB2019_main_1214_essence_1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
main.cpp@0:6212b283430c, 2019-10-27 (annotated)
- Committer:
- YUPPY
- Date:
- Sun Oct 27 05:20:43 2019 +0000
- Revision:
- 0:6212b283430c
- Child:
- 1:edc264954800
main ver.2;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| YUPPY | 0:6212b283430c | 1 | #define cansatB |
| YUPPY | 0:6212b283430c | 2 | #include “mbed.h” //mbed |
| YUPPY | 0:6212b283430c | 3 | #include “getgps.h” //GPS |
| YUPPY | 0:6212b283430c | 4 | #include “motordriver.h” //モータードライバ |
| YUPPY | 0:6212b283430c | 5 | #include “XBee.h” //XBee |
| YUPPY | 0:6212b283430c | 6 | #include <SoftwareSerial.h> //カメラ |
| YUPPY | 0:6212b283430c | 7 | #include <SD.h>//SDカード |
| YUPPY | 0:6212b283430c | 8 | #include <JPEGCamera.h>//カメラ |
| YUPPY | 0:6212b283430c | 9 | #include “US015.h” // 超音波センサ |
| YUPPY | 0:6212b283430c | 10 | #include "sb612a.h"//焦電センサー |
| YUPPY | 0:6212b283430c | 11 | #include <stdio.h> |
| YUPPY | 0:6212b283430c | 12 | #include <wiringPi.h> |
| YUPPY | 0:6212b283430c | 13 | #include <mcp23s17.h> |
| YUPPY | 0:6212b283430c | 14 | #include <sys/time.h> |
| YUPPY | 0:6212b283430c | 15 | #include <unistd.h> |
| YUPPY | 0:6212b283430c | 16 | DigitalOut USSTriger (p12); //P12 :超音波センサ トリガ出力 |
| YUPPY | 0:6212b283430c | 17 | InterruptIn USSEcho (p11); //p11 :超音波センサ エコー入力 |
| YUPPY | 0:6212b283430c | 18 | Serial pc(USBTX, USBRX); //GPS |
| YUPPY | 0:6212b283430c | 19 | GPS gps(p28, p27); //GPS |
| YUPPY | 0:6212b283430c | 20 | PwmOut motorSpeedR(p26); //モーター |
| YUPPY | 0:6212b283430c | 21 | PwmOut motorSpeedL(p25); //モーター |
| YUPPY | 0:6212b283430c | 22 | DigitalIn flight(p22,p23); //フライトピン |
| YUPPY | 0:6212b283430c | 23 | DigitalOut FET(p21); //FET |
| YUPPY | 0:6212b283430c | 24 | Timer ActiveTime; |
| YUPPY | 0:6212b283430c | 25 | XBee xbee(p13, p14); // XBee |
| YUPPY | 0:6212b283430c | 26 | AnalogIn (p15,p16); //焦電センサ |
| YUPPY | 0:6212b283430c | 27 | Serial pc (p9); //カメラ |
| YUPPY | 0:6212b283430c | 28 | //Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター |
| YUPPY | 0:6212b283430c | 29 | //Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake |
| YUPPY | 0:6212b283430c | 30 | void dist(int distance) |
| YUPPY | 0:6212b283430c | 31 | { |
| YUPPY | 0:6212b283430c | 32 | //put code here to happen when the distance is changed |
| YUPPY | 0:6212b283430c | 33 | printf(“Distance changed to %dmm\r\n”, distance); |
| YUPPY | 0:6212b283430c | 34 | } |
| YUPPY | 0:6212b283430c | 35 | printf(“GPS start\r\n”);//GPS 第一回目 |
| YUPPY | 0:6212b283430c | 36 | FILE *fp = fopen(“/local/gps,foto.txt”, “w”); // Open “gps.txt” on the local file system for writing |
| YUPPY | 0:6212b283430c | 37 | fprintf(fp, “GPS Start\r\n”); |
| YUPPY | 0:6212b283430c | 38 | int n; |
| YUPPY | 0:6212b283430c | 39 | for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半) |
| YUPPY | 0:6212b283430c | 40 | { |
| YUPPY | 0:6212b283430c | 41 | printf(“gps for\r\n”); |
| YUPPY | 0:6212b283430c | 42 | if(gps.getgps()) //現在地取得 |
| YUPPY | 0:6212b283430c | 43 | fprintf(fp,“(%lf, %lf)\r\n”, gps.latitude, gps.longitude);//緯度と経度を出力 |
| YUPPY | 0:6212b283430c | 44 | else |
| YUPPY | 0:6212b283430c | 45 | fprintf(fp,“No data\r\n”);//データ取得に失敗した場合 |
| YUPPY | 0:6212b283430c | 46 | wait(1); |
| YUPPY | 0:6212b283430c | 47 | printf(“%d\r\n”,n); //今何回目かを出力（本番ではいらない） |
| YUPPY | 0:6212b283430c | 48 | } |
| YUPPY | 0:6212b283430c | 49 | fprintf(fp,“GPS finish\r\n”); |
| YUPPY | 0:6212b283430c | 50 | // fclose(fp); // GPSの測定終了 */ |
| YUPPY | 0:6212b283430c | 51 | US015 mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 超音波センサー |
| YUPPY | 0:6212b283430c | 52 | //have updates every .1 seconds and a timeout after 1 |
| YUPPY | 0:6212b283430c | 53 | //second, and call dist when the distance changes |
| YUPPY | 0:6212b283430c | 54 | void motorForwardR(void); |
| YUPPY | 0:6212b283430c | 55 | void motorStopR(void); |
| YUPPY | 0:6212b283430c | 56 | void motorReverseR(void); |
| YUPPY | 0:6212b283430c | 57 | void motorForwardL(void); |
| YUPPY | 0:6212b283430c | 58 | void motorStopL(void); |
| YUPPY | 0:6212b283430c | 59 | void motorReverseL(void); |
| YUPPY | 0:6212b283430c | 60 | LocalFileSystem local(“local”); // Create the local filesystem under the name “local” データ保存 |
| YUPPY | 0:6212b283430c | 61 | float culculate_distance_3(float a,float b); |
| YUPPY | 0:6212b283430c | 62 | int main() { |
| YUPPY | 0:6212b283430c | 63 | printf(“cansat start\r\n”); |
| YUPPY | 0:6212b283430c | 64 | flight==1; |
| YUPPY | 0:6212b283430c | 65 | FET = 0; |
| YUPPY | 0:6212b283430c | 66 | SW = 1; //p23をhigh(3.3V)にする。 |
| YUPPY | 0:6212b283430c | 67 | while(1) { |
| YUPPY | 0:6212b283430c | 68 | if(flight==1) { |
| YUPPY | 0:6212b283430c | 69 | wait(10); |
| YUPPY | 0:6212b283430c | 70 | printf(“mada\r\n”); |
| YUPPY | 0:6212b283430c | 71 | } |
| YUPPY | 0:6212b283430c | 72 | else{ |
| YUPPY | 0:6212b283430c | 73 | if(flight==1) { |
| YUPPY | 0:6212b283430c | 74 | wait(10); |
| YUPPY | 0:6212b283430c | 75 | printf(“madamada\r\n”); |
| YUPPY | 0:6212b283430c | 76 | } |
| YUPPY | 0:6212b283430c | 77 | else{ |
| YUPPY | 0:6212b283430c | 78 | printf(“yattar\r\n”); |
| YUPPY | 0:6212b283430c | 79 | FET = 0; //FET、ニクロム線切断 |
| YUPPY | 0:6212b283430c | 80 | wait(20); |
| YUPPY | 0:6212b283430c | 81 | FET = 1; |
| YUPPY | 0:6212b283430c | 82 | wait(12); |
| YUPPY | 0:6212b283430c | 83 | FET = 0; |
| YUPPY | 0:6212b283430c | 84 | wait(10); |
| YUPPY | 0:6212b283430c | 85 | FET = 1; |
| YUPPY | 0:6212b283430c | 86 | wait(12); |
| YUPPY | 0:6212b283430c | 87 | FET = 0; |
| YUPPY | 0:6212b283430c | 88 | SW = 0; //p23をlow(0V)にする。 |
| YUPPY | 0:6212b283430c | 89 | break; |
| YUPPY | 0:6212b283430c | 90 | } |
| YUPPY | 0:6212b283430c | 91 | } |
| YUPPY | 0:6212b283430c | 92 | } |
| YUPPY | 0:6212b283430c | 93 | motorStopR(); |
| YUPPY | 0:6212b283430c | 94 | motorStopL(); |
| YUPPY | 0:6212b283430c | 95 | wait(20); //確認用//デフォ20秒 |
| YUPPY | 0:6212b283430c | 96 | // FILE *fp = fopen(“/local/gps.txt”, “w”); // Open “gps.txt” on the local file system for writing |
| YUPPY | 0:6212b283430c | 97 | motorSpeedR.period_ms(4); //モーター調節 |
| YUPPY | 0:6212b283430c | 98 | motorSpeedR = 0.655; |
| YUPPY | 0:6212b283430c | 99 | motorSpeedL.period_ms(4); //モーター調節 |
| YUPPY | 0:6212b283430c | 100 | motorSpeedL = 0.755; |
| YUPPY | 0:6212b283430c | 101 | motorForwardL(); //車体を時計回りに３秒回転 |
| YUPPY | 0:6212b283430c | 102 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 103 | wait(1.6); |
| YUPPY | 0:6212b283430c | 104 | motorStopR(); |
| YUPPY | 0:6212b283430c | 105 | motorStopL(); |
| YUPPY | 0:6212b283430c | 106 | wait(1); |
| YUPPY | 0:6212b283430c | 107 | motorReverseL(); //車体を反時計回りに３秒回転 |
| YUPPY | 0:6212b283430c | 108 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 109 | wait(1.6); |
| YUPPY | 0:6212b283430c | 110 | motorStopR(); |
| YUPPY | 0:6212b283430c | 111 | motorStopL(); |
| YUPPY | 0:6212b283430c | 112 | wait(1); |
| YUPPY | 0:6212b283430c | 113 | printf(“compass carriblation\r\n”); //キャリブレーション終了 |
| YUPPY | 0:6212b283430c | 114 | //float mcn1,mcn2; |
| YUPPY | 0:6212b283430c | 115 | printf(“GPS start\r\n”);//GPS第２回目（移動後） |
| YUPPY | 0:6212b283430c | 116 | FILE *fp = fopen(“/local/gps,foto.txt”, “w”); // Open “gps.txt” on the local file system for writing |
| YUPPY | 0:6212b283430c | 117 | fprintf(fp, “GPS Start\r\n”); |
| YUPPY | 0:6212b283430c | 118 | int n; |
| YUPPY | 0:6212b283430c | 119 | for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半) |
| YUPPY | 0:6212b283430c | 120 | { |
| YUPPY | 0:6212b283430c | 121 | printf(“gps for\r\n”); |
| YUPPY | 0:6212b283430c | 122 | if(gps.getgps()) //現在地取得 |
| YUPPY | 0:6212b283430c | 123 | fprintf(fp,“(%lf, %lf)\r\n”, gps.latitude, gps.longitude);//緯度と経度を出力 |
| YUPPY | 0:6212b283430c | 124 | else |
| YUPPY | 0:6212b283430c | 125 | fprintf(fp,“No data\r\n”);//データ取得に失敗した場合 |
| YUPPY | 0:6212b283430c | 126 | wait(1); |
| YUPPY | 0:6212b283430c | 127 | printf(“%d\r\n”,n); //今何回目かを出力（本番ではいらない） |
| YUPPY | 0:6212b283430c | 128 | } |
| YUPPY | 0:6212b283430c | 129 | fprintf(fp,“GPS finish\r\n”); |
| YUPPY | 0:6212b283430c | 130 | // fclose(fp); // GPSの測定終了 */ |
| YUPPY | 0:6212b283430c | 131 | |
| YUPPY | 0:6212b283430c | 132 | if |
| YUPPY | 0:6212b283430c | 133 | //超音波センサー反応あり |
| YUPPY | 0:6212b283430c | 134 | //停止 |
| YUPPY | 0:6212b283430c | 135 | //超音波センサーOFF |
| YUPPY | 0:6212b283430c | 136 | //焦電センサーON |
| YUPPY | 0:6212b283430c | 137 | if |
| YUPPY | 0:6212b283430c | 138 | //焦電センサー反応あり |
| YUPPY | 0:6212b283430c | 139 | //焦電センサーOFF |
| YUPPY | 0:6212b283430c | 140 | //カメラ起動 |
| YUPPY | 0:6212b283430c | 141 | //撮影 |
| YUPPY | 0:6212b283430c | 142 | //カメラOFF |
| YUPPY | 0:6212b283430c | 143 | //データをSDカードに保存 |
| YUPPY | 0:6212b283430c | 144 | //保存データをXBeeによりPCへ送信 |
| YUPPY | 0:6212b283430c | 145 | if |
| YUPPY | 0:6212b283430c | 146 | //"OK"受信、ミッション終了 |
| YUPPY | 0:6212b283430c | 147 | else if |
| YUPPY | 0:6212b283430c | 148 | //"NO"受信、ミッション再開 |
| YUPPY | 0:6212b283430c | 149 | |
| YUPPY | 0:6212b283430c | 150 | else if |
| YUPPY | 0:6212b283430c | 151 | //焦電センサー反応無し |
| YUPPY | 0:6212b283430c | 152 | //焦電センサーOFF |
| YUPPY | 0:6212b283430c | 153 | //方向転換 |
| YUPPY | 0:6212b283430c | 154 | //超音波センサーON |
| YUPPY | 0:6212b283430c | 155 | //直進 |
| YUPPY | 0:6212b283430c | 156 | else if |
| YUPPY | 0:6212b283430c | 157 | //超音波センサー反応無し |
| YUPPY | 0:6212b283430c | 158 | //停止 |
| YUPPY | 0:6212b283430c | 159 | //方向転換 |
| YUPPY | 0:6212b283430c | 160 | //直進 |
| YUPPY | 0:6212b283430c | 161 | |
| YUPPY | 0:6212b283430c | 162 | |
| YUPPY | 0:6212b283430c | 163 | |
| YUPPY | 0:6212b283430c | 164 | |
| YUPPY | 0:6212b283430c | 165 | |
| YUPPY | 0:6212b283430c | 166 | |
| YUPPY | 0:6212b283430c | 167 | double heading,wx,wy,wz; |
| YUPPY | 0:6212b283430c | 168 | // mcn1=1.0; |
| YUPPY | 0:6212b283430c | 169 | // mcn2=1.0; |
| YUPPY | 0:6212b283430c | 170 | heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する |
| YUPPY | 0:6212b283430c | 171 | if(90<heading<267.5){ |
| YUPPY | 0:6212b283430c | 172 | printf(“right\r\n”); |
| YUPPY | 0:6212b283430c | 173 | motorForwardL();//右回転 |
| YUPPY | 0:6212b283430c | 174 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 175 | wx=(270-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 176 | wait(wx); //角度のずれ*1度回転するのにかかる時間 |
| YUPPY | 0:6212b283430c | 177 | motorStopR(); |
| YUPPY | 0:6212b283430c | 178 | motorStopL(); |
| YUPPY | 0:6212b283430c | 179 | wait(1); |
| YUPPY | 0:6212b283430c | 180 | } |
| YUPPY | 0:6212b283430c | 181 | else if(0<=heading<=90.0){ |
| YUPPY | 0:6212b283430c | 182 | printf(“left\r\n”); |
| YUPPY | 0:6212b283430c | 183 | motorForwardL();//左回転 |
| YUPPY | 0:6212b283430c | 184 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 185 | wy=(270-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 186 | wait(wy); |
| YUPPY | 0:6212b283430c | 187 | motorStopR(); |
| YUPPY | 0:6212b283430c | 188 | motorStopL(); |
| YUPPY | 0:6212b283430c | 189 | wait(1); |
| YUPPY | 0:6212b283430c | 190 | } |
| YUPPY | 0:6212b283430c | 191 | else if(272.5<heading<360){ |
| YUPPY | 0:6212b283430c | 192 | printf(“left\r\n”); |
| YUPPY | 0:6212b283430c | 193 | motorForwardL();//左回転 |
| YUPPY | 0:6212b283430c | 194 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 195 | wz=(360-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 196 | wait(wz); |
| YUPPY | 0:6212b283430c | 197 | motorStopR(); |
| YUPPY | 0:6212b283430c | 198 | motorStopL(); |
| YUPPY | 0:6212b283430c | 199 | wait(1); |
| YUPPY | 0:6212b283430c | 200 | } |
| YUPPY | 0:6212b283430c | 201 | else{ |
| YUPPY | 0:6212b283430c | 202 | wait(5); |
| YUPPY | 0:6212b283430c | 203 | } |
| YUPPY | 0:6212b283430c | 204 | printf(“searchN\r\n”); //機体が北を向く |
| YUPPY | 0:6212b283430c | 205 | wait(2); |
| YUPPY | 0:6212b283430c | 206 | mu.startUpdates();//start mesuring the distance(超音波センサー) |
| YUPPY | 0:6212b283430c | 207 | int distance; |
| YUPPY | 0:6212b283430c | 208 | int flag=0,flag2=0; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、 |
| YUPPY | 0:6212b283430c | 209 | //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから |
| YUPPY | 0:6212b283430c | 210 | //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。 |
| YUPPY | 0:6212b283430c | 211 | float rightrun; //変数runをフロートで型づけする |
| YUPPY | 0:6212b283430c | 212 | float leftrun; |
| YUPPY | 0:6212b283430c | 213 | rightrun=0.0; |
| YUPPY | 0:6212b283430c | 214 | leftrun=0.0; |
| YUPPY | 0:6212b283430c | 215 | int accel[3];//accelを3つの配列で定義。*/ |
| YUPPY | 0:6212b283430c | 216 | int gyro[3]; |
| YUPPY | 0:6212b283430c | 217 | int tt=0; |
| YUPPY | 0:6212b283430c | 218 | float run=0; |
| YUPPY | 0:6212b283430c | 219 | fprintf(fp, “x,y,z,lefttrun2,rightrun\r\n”); |
| YUPPY | 0:6212b283430c | 220 | int k=0;//中間地点角度判定プログラムを１回しか通さないためのフラグ |
| YUPPY | 0:6212b283430c | 221 | while(1) |
| YUPPY | 0:6212b283430c | 222 | { |
| YUPPY | 0:6212b283430c | 223 | distance=mu.getCurrentDistance(); |
| YUPPY | 0:6212b283430c | 224 | printf(“%d\r\n”,distance); |
| YUPPY | 0:6212b283430c | 225 | mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 |
| YUPPY | 0:6212b283430c | 226 | int x = accel[0];//x軸方向の加速度 |
| YUPPY | 0:6212b283430c | 227 | int y = accel[1];//y軸方向の加速度 |
| YUPPY | 0:6212b283430c | 228 | int z = accel[2] ;//z軸方向の加速度 |
| YUPPY | 0:6212b283430c | 229 | mpu.readGyroData(gyro);//加速度の値をaccel[3]に代入 |
| YUPPY | 0:6212b283430c | 230 | int gx = gyro[0];//x軸方向の加速度 |
| YUPPY | 0:6212b283430c | 231 | int gy = gyro[1];//y軸方向の加速度 |
| YUPPY | 0:6212b283430c | 232 | int gz = gyro[2];//z軸方向の加速度 |
| YUPPY | 0:6212b283430c | 233 | printf(“x=%d,y=%d,z=%d\r\n”,x,y,z); //加速度の表示 |
| YUPPY | 0:6212b283430c | 234 | printf(“%d\r\n”, test.read()); //フォトインタラプタ |
| YUPPY | 0:6212b283430c | 235 | printf(“%d\r\n”, test2.read()); |
| YUPPY | 0:6212b283430c | 236 | if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは |
| YUPPY | 0:6212b283430c | 237 | flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま |
| YUPPY | 0:6212b283430c | 238 | printf(“test.read if\r\n”); |
| YUPPY | 0:6212b283430c | 239 | } |
| YUPPY | 0:6212b283430c | 240 | else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは |
| YUPPY | 0:6212b283430c | 241 | flag = 0; //まずこれでスイッチを0にして入れる。 |
| YUPPY | 0:6212b283430c | 242 | //こうすることで同じスリットの中でtestが複数回0を返した時に何回も56.5mmを加算しつづけるということがなくなる |
| YUPPY | 0:6212b283430c | 243 | rightrun += 113; //総距離runに113mmを加算する(タイヤの全周452mm) |
| YUPPY | 0:6212b283430c | 244 | printf(“test.read else\r\n”); |
| YUPPY | 0:6212b283430c | 245 | } |
| YUPPY | 0:6212b283430c | 246 | if (test2.read() == 1 and flag2 == 0){ |
| YUPPY | 0:6212b283430c | 247 | flag2 = 1; |
| YUPPY | 0:6212b283430c | 248 | printf(“test2.read if\r\n”); |
| YUPPY | 0:6212b283430c | 249 | } |
| YUPPY | 0:6212b283430c | 250 | else if (test2.read() == 0 and flag2 == 1){ |
| YUPPY | 0:6212b283430c | 251 | flag2 = 0; |
| YUPPY | 0:6212b283430c | 252 | leftrun += 113; |
| YUPPY | 0:6212b283430c | 253 | printf(“test2.read else\r\n”); |
| YUPPY | 0:6212b283430c | 254 | } |
| YUPPY | 0:6212b283430c | 255 | printf(“%f %g rot “, leftrun,leftrun/452); |
| YUPPY | 0:6212b283430c | 256 | printf(“%f %g rot\r\n”, rightrun,rightrun/452); |
| YUPPY | 0:6212b283430c | 257 | run=culculate_distance_3(rightrun,leftrun); |
| YUPPY | 0:6212b283430c | 258 | if(run >= 30000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する |
| YUPPY | 0:6212b283430c | 259 | break; //つまりゴールについたらこのループからぬける |
| YUPPY | 0:6212b283430c | 260 | } |
| YUPPY | 0:6212b283430c | 261 | else if(15000< run && run<15050 && k!=1){//中間地点で北を向きなおす |
| YUPPY | 0:6212b283430c | 262 | motorStopR(); |
| YUPPY | 0:6212b283430c | 263 | motorStopL(); |
| YUPPY | 0:6212b283430c | 264 | wait(1); |
| YUPPY | 0:6212b283430c | 265 | heading=compass.getHeadingXYDeg();//コンパスの値をもう一度headingに格納 |
| YUPPY | 0:6212b283430c | 266 | wait(2); |
| YUPPY | 0:6212b283430c | 267 | k=1; |
| YUPPY | 0:6212b283430c | 268 | heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する |
| YUPPY | 0:6212b283430c | 269 | if(90<heading<267.5){ |
| YUPPY | 0:6212b283430c | 270 | printf(“right\r\n”); |
| YUPPY | 0:6212b283430c | 271 | motorForwardL();//右回転 |
| YUPPY | 0:6212b283430c | 272 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 273 | wx=(270-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 274 | wait(wx); //角度のずれ*1度回転するのにかかる時間 |
| YUPPY | 0:6212b283430c | 275 | motorStopR(); |
| YUPPY | 0:6212b283430c | 276 | motorStopL(); |
| YUPPY | 0:6212b283430c | 277 | wait(1); |
| YUPPY | 0:6212b283430c | 278 | } |
| YUPPY | 0:6212b283430c | 279 | else if(0<=heading<=90.0){ |
| YUPPY | 0:6212b283430c | 280 | printf(“left\r\n”); |
| YUPPY | 0:6212b283430c | 281 | motorForwardL();//左回転 |
| YUPPY | 0:6212b283430c | 282 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 283 | wy=(270-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 284 | wait(wy); |
| YUPPY | 0:6212b283430c | 285 | motorStopR(); |
| YUPPY | 0:6212b283430c | 286 | motorStopL(); |
| YUPPY | 0:6212b283430c | 287 | wait(1); |
| YUPPY | 0:6212b283430c | 288 | } |
| YUPPY | 0:6212b283430c | 289 | else if(272.5<heading<360){ |
| YUPPY | 0:6212b283430c | 290 | printf(“left\r\n”); |
| YUPPY | 0:6212b283430c | 291 | motorForwardL();//左回転 |
| YUPPY | 0:6212b283430c | 292 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 293 | wz=(360-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 294 | wait(wz); |
| YUPPY | 0:6212b283430c | 295 | motorStopR(); |
| YUPPY | 0:6212b283430c | 296 | motorStopL(); |
| YUPPY | 0:6212b283430c | 297 | wait(1); |
| YUPPY | 0:6212b283430c | 298 | } |
| YUPPY | 0:6212b283430c | 299 | else{ |
| YUPPY | 0:6212b283430c | 300 | wait(5); |
| YUPPY | 0:6212b283430c | 301 | } |
| YUPPY | 0:6212b283430c | 302 | printf(“searchN\r\n”); //機体が北を向く |
| YUPPY | 0:6212b283430c | 303 | } |
| YUPPY | 0:6212b283430c | 304 | /* if(difference >= 0.3) |
| YUPPY | 0:6212b283430c | 305 | { |
| YUPPY | 0:6212b283430c | 306 | break; |
| YUPPY | 0:6212b283430c | 307 | } */ |
| YUPPY | 0:6212b283430c | 308 | motorForwardL(); //通常走行 |
| YUPPY | 0:6212b283430c | 309 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 310 | //Do something else here |
| YUPPY | 0:6212b283430c | 311 | // mu.checkDistance(); //call checkDistance() as much as possible, as this is where |
| YUPPY | 0:6212b283430c | 312 | //the class checks if dist needs to be called. |
| YUPPY | 0:6212b283430c | 313 | wait(0.01); |
| YUPPY | 0:6212b283430c | 314 | fprintf(fp,“%5d, %5d, %5d, %5d, %5d, %5d, %8lf, %8lf\r\n”, x, y, z,gx,gy,gz, leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力 |
| YUPPY | 0:6212b283430c | 315 | if(450 < distance && distance < 550) //障害物発見 |
| YUPPY | 0:6212b283430c | 316 | { |
| YUPPY | 0:6212b283430c | 317 | printf(“if success!\r\n”); |
| YUPPY | 0:6212b283430c | 318 | float ms1,ms2,msj1,msj2; |
| YUPPY | 0:6212b283430c | 319 | ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ |
| YUPPY | 0:6212b283430c | 320 | ms2=1.0; |
| YUPPY | 0:6212b283430c | 321 | msj1=1.0; //回転の時 |
| YUPPY | 0:6212b283430c | 322 | msj2=1.0; |
| YUPPY | 0:6212b283430c | 323 | motorStopR(); |
| YUPPY | 0:6212b283430c | 324 | motorStopL(); |
| YUPPY | 0:6212b283430c | 325 | wait(2); |
| YUPPY | 0:6212b283430c | 326 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 327 | motorForwardL(); //機体を時計回りに９０度回転 |
| YUPPY | 0:6212b283430c | 328 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 329 | wait(1); |
| YUPPY | 0:6212b283430c | 330 | printf(“mortor rotation\r\n”); |
| YUPPY | 0:6212b283430c | 331 | motorStopR(); |
| YUPPY | 0:6212b283430c | 332 | motorStopL(); |
| YUPPY | 0:6212b283430c | 333 | wait(2); |
| YUPPY | 0:6212b283430c | 334 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 335 | motorForwardL(); //直進 |
| YUPPY | 0:6212b283430c | 336 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 337 | wait(3); |
| YUPPY | 0:6212b283430c | 338 | printf(“mortor straight\r\n”); |
| YUPPY | 0:6212b283430c | 339 | motorStopR(); |
| YUPPY | 0:6212b283430c | 340 | motorStopL(); |
| YUPPY | 0:6212b283430c | 341 | wait(2); |
| YUPPY | 0:6212b283430c | 342 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 343 | motorReverseL(); //機体を反時計回りに９０度回転 |
| YUPPY | 0:6212b283430c | 344 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 345 | wait(1); |
| YUPPY | 0:6212b283430c | 346 | printf(“mortor rotation\r\n”); |
| YUPPY | 0:6212b283430c | 347 | motorStopR(); |
| YUPPY | 0:6212b283430c | 348 | motorStopL(); |
| YUPPY | 0:6212b283430c | 349 | wait(2); |
| YUPPY | 0:6212b283430c | 350 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 351 | motorForwardL(); //直進 |
| YUPPY | 0:6212b283430c | 352 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 353 | int t=0; |
| YUPPY | 0:6212b283430c | 354 | for(t=0;t<100;t++) |
| YUPPY | 0:6212b283430c | 355 | { |
| YUPPY | 0:6212b283430c | 356 | printf(“%d\r\n”, test.read()); |
| YUPPY | 0:6212b283430c | 357 | printf(“%d\r\n”, test2.read()); |
| YUPPY | 0:6212b283430c | 358 | if (test.read() == 1 and flag == 0) |
| YUPPY | 0:6212b283430c | 359 | { |
| YUPPY | 0:6212b283430c | 360 | flag = 1; |
| YUPPY | 0:6212b283430c | 361 | } |
| YUPPY | 0:6212b283430c | 362 | else if (test.read() == 0 and flag == 1) |
| YUPPY | 0:6212b283430c | 363 | { |
| YUPPY | 0:6212b283430c | 364 | flag=0; |
| YUPPY | 0:6212b283430c | 365 | rightrun += 113; |
| YUPPY | 0:6212b283430c | 366 | } |
| YUPPY | 0:6212b283430c | 367 | if (test2.read() == 1 and flag2 == 0){ |
| YUPPY | 0:6212b283430c | 368 | flag2 = 1; |
| YUPPY | 0:6212b283430c | 369 | } |
| YUPPY | 0:6212b283430c | 370 | else if (test2.read() == 0 and flag2 == 1){ |
| YUPPY | 0:6212b283430c | 371 | flag2 = 0; |
| YUPPY | 0:6212b283430c | 372 | leftrun += 113; |
| YUPPY | 0:6212b283430c | 373 | } |
| YUPPY | 0:6212b283430c | 374 | printf(“%f”, leftrun); |
| YUPPY | 0:6212b283430c | 375 | printf(“\t%f\r\n”, rightrun); |
| YUPPY | 0:6212b283430c | 376 | mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 |
| YUPPY | 0:6212b283430c | 377 | mpu.readGyroData(gyro); |
| YUPPY | 0:6212b283430c | 378 | int x = accel[0]; |
| YUPPY | 0:6212b283430c | 379 | int y = accel[1]; |
| YUPPY | 0:6212b283430c | 380 | int z = accel[2]; |
| YUPPY | 0:6212b283430c | 381 | int gx = gyro[0]; |
| YUPPY | 0:6212b283430c | 382 | int gy = gyro[1]; |
| YUPPY | 0:6212b283430c | 383 | int gz = gyro[2]; |
| YUPPY | 0:6212b283430c | 384 | fprintf(fp,“%5d, %5d, %5d, %8lf, %8lf, %5d, %5d, %5d\r\n”, x, y, z, leftrun, rightrun,gx, gy ,gz);//加速度とフォトインタラプタによる距離を出力 |
| YUPPY | 0:6212b283430c | 385 | if (run >= 300000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する |
| YUPPY | 0:6212b283430c | 386 | break; //つまりゴールについたらこのループからぬける |
| YUPPY | 0:6212b283430c | 387 | } |
| YUPPY | 0:6212b283430c | 388 | wait(0.01); |
| YUPPY | 0:6212b283430c | 389 | } |
| YUPPY | 0:6212b283430c | 390 | printf(“mortor straight\r\n”); |
| YUPPY | 0:6212b283430c | 391 | motorStopR(); |
| YUPPY | 0:6212b283430c | 392 | motorStopL(); |
| YUPPY | 0:6212b283430c | 393 | wait(2); |
| YUPPY | 0:6212b283430c | 394 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 395 | motorReverseL(); //機体を反時計回りに９０度回転 |
| YUPPY | 0:6212b283430c | 396 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 397 | wait(1); |
| YUPPY | 0:6212b283430c | 398 | printf(“mortor rotation\r\n”); |
| YUPPY | 0:6212b283430c | 399 | motorStopR(); |
| YUPPY | 0:6212b283430c | 400 | motorStopL(); |
| YUPPY | 0:6212b283430c | 401 | wait(2); |
| YUPPY | 0:6212b283430c | 402 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 403 | motorForwardL(); //直進 |
| YUPPY | 0:6212b283430c | 404 | motorForwardR(); |
| YUPPY | 0:6212b283430c | 405 | wait(2); |
| YUPPY | 0:6212b283430c | 406 | printf(“mortor straight\r\n”); |
| YUPPY | 0:6212b283430c | 407 | motorStopR(); |
| YUPPY | 0:6212b283430c | 408 | motorStopL(); |
| YUPPY | 0:6212b283430c | 409 | wait(2); |
| YUPPY | 0:6212b283430c | 410 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 411 | motorForwardL(); //機体を時計回りに９０度回転 |
| YUPPY | 0:6212b283430c | 412 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 413 | wait(1); |
| YUPPY | 0:6212b283430c | 414 | printf(“mortor rotation\r\n”); |
| YUPPY | 0:6212b283430c | 415 | motorStopR(); |
| YUPPY | 0:6212b283430c | 416 | motorStopL(); |
| YUPPY | 0:6212b283430c | 417 | wait(2); |
| YUPPY | 0:6212b283430c | 418 | printf(“mortor stop\r\n”); |
| YUPPY | 0:6212b283430c | 419 | heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する |
| YUPPY | 0:6212b283430c | 420 | if(90<heading<267.5){ |
| YUPPY | 0:6212b283430c | 421 | printf(“right\r\n”); |
| YUPPY | 0:6212b283430c | 422 | motorForwardL();//右回転 |
| YUPPY | 0:6212b283430c | 423 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 424 | wx=(270-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 425 | wait(wx); //角度のずれ*1度回転するのにかかる時間 |
| YUPPY | 0:6212b283430c | 426 | motorStopR(); |
| YUPPY | 0:6212b283430c | 427 | motorStopL(); |
| YUPPY | 0:6212b283430c | 428 | wait(1); |
| YUPPY | 0:6212b283430c | 429 | } |
| YUPPY | 0:6212b283430c | 430 | else if(0<=heading<=90.0){ |
| YUPPY | 0:6212b283430c | 431 | printf(“left\r\n”); |
| YUPPY | 0:6212b283430c | 432 | motorForwardL();//左回転 |
| YUPPY | 0:6212b283430c | 433 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 434 | wy=(270-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 435 | wait(wy); |
| YUPPY | 0:6212b283430c | 436 | motorStopR(); |
| YUPPY | 0:6212b283430c | 437 | motorStopL(); |
| YUPPY | 0:6212b283430c | 438 | wait(1); |
| YUPPY | 0:6212b283430c | 439 | } |
| YUPPY | 0:6212b283430c | 440 | else if(272.5<heading<360){ |
| YUPPY | 0:6212b283430c | 441 | printf(“left\r\n”); |
| YUPPY | 0:6212b283430c | 442 | motorForwardL();//左回転 |
| YUPPY | 0:6212b283430c | 443 | motorReverseR(); |
| YUPPY | 0:6212b283430c | 444 | wz=(360-heading)*0.004448; |
| YUPPY | 0:6212b283430c | 445 | wait(wz); |
| YUPPY | 0:6212b283430c | 446 | motorStopR(); |
| YUPPY | 0:6212b283430c | 447 | motorStopL(); |
| YUPPY | 0:6212b283430c | 448 | wait(1); |
| YUPPY | 0:6212b283430c | 449 | } |
| YUPPY | 0:6212b283430c | 450 | else{ |
| YUPPY | 0:6212b283430c | 451 | wait(5); |
| YUPPY | 0:6212b283430c | 452 | } |
| YUPPY | 0:6212b283430c | 453 | printf(“searchN\r\n”); //機体が北を向く |
| YUPPY | 0:6212b283430c | 454 | } |
| YUPPY | 0:6212b283430c | 455 | } |
| YUPPY | 0:6212b283430c | 456 | motorStopR(); |
| YUPPY | 0:6212b283430c | 457 | motorStopL(); |
| YUPPY | 0:6212b283430c | 458 | fprintf(fp, “last photo\r\n”); |
| YUPPY | 0:6212b283430c | 459 | fprintf(fp,“(%lf)\r\n”, run);//最後の加速度とフォトインタラプタによる距離を出力 |
| YUPPY | 0:6212b283430c | 460 | fprintf(fp, “last left.right\r\n”); |
| YUPPY | 0:6212b283430c | 461 | fprintf(fp,“(%lf, %lf)\r\n”, leftrun, rightrun); |
| YUPPY | 0:6212b283430c | 462 | fclose(fp); |
| YUPPY | 0:6212b283430c | 463 | } |
| YUPPY | 0:6212b283430c | 464 | float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる |
| YUPPY | 0:6212b283430c | 465 | { |
| YUPPY | 0:6212b283430c | 466 | float c; |
| YUPPY | 0:6212b283430c | 467 | c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える |
| YUPPY | 0:6212b283430c | 468 | return c; |
| YUPPY | 0:6212b283430c | 469 | } |
| YUPPY | 0:6212b283430c | 470 | void motorForwardR() { |
| YUPPY | 0:6212b283430c | 471 | motorStopR(); |
| YUPPY | 0:6212b283430c | 472 | motor1Dir1 = 1; |
| YUPPY | 0:6212b283430c | 473 | motor1Dir2 = 0; |
| YUPPY | 0:6212b283430c | 474 | } |
| YUPPY | 0:6212b283430c | 475 | void motorReverseR() { |
| YUPPY | 0:6212b283430c | 476 | motorStopR(); |
| YUPPY | 0:6212b283430c | 477 | motor1Dir1 = 0; |
| YUPPY | 0:6212b283430c | 478 | motor1Dir2 = 1; |
| YUPPY | 0:6212b283430c | 479 | } |
| YUPPY | 0:6212b283430c | 480 | void motorStopR() { |
| YUPPY | 0:6212b283430c | 481 | motor1Dir1 = 0; |
| YUPPY | 0:6212b283430c | 482 | motor1Dir2 = 0; |
| YUPPY | 0:6212b283430c | 483 | } |
| YUPPY | 0:6212b283430c | 484 | void motorForwardL() { |
| YUPPY | 0:6212b283430c | 485 | motorStopR(); |
| YUPPY | 0:6212b283430c | 486 | motor2Dir1 = 1; |
| YUPPY | 0:6212b283430c | 487 | motor2Dir2 = 0; |
| YUPPY | 0:6212b283430c | 488 | } |
| YUPPY | 0:6212b283430c | 489 | void motorReverseL() { |
| YUPPY | 0:6212b283430c | 490 | motorStopR(); |
| YUPPY | 0:6212b283430c | 491 | motor2Dir1 = 0; |
| YUPPY | 0:6212b283430c | 492 | motor2Dir2 = 1; |
| YUPPY | 0:6212b283430c | 493 | } |
| YUPPY | 0:6212b283430c | 494 | void motorStopL() { |
| YUPPY | 0:6212b283430c | 495 | motor2Dir1 = 0; |
| YUPPY | 0:6212b283430c | 496 | motor2Dir2 = 0; |
| YUPPY | 0:6212b283430c | 497 | } |