悠登 山本 / Mbed 2 deprecated CanSatB2019_main_Class導入前

Class導入前です　まだできてません

Dependencies:   mbed CameraUS015sb612-3

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?

UserRevisionLine numberNew 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 }