main.cpp

00001 //コマンドなしver
00002 //コマンドだと反応しないため→コマンド用のxbeeを必要だと思われる。
00003 //xbeeが使えない→基盤ミス
00004 
00005 #include "mbed.h"
00006 #include "SRF02.h"
00007 #include "SDFileSystem.h"
00008 #include "Servo.h"
00009 #include "MPU6050.h"
00010 #include "LPS.h"
00011 
00012 DigitalOut myleds[]={LED1,LED2,LED3,LED4};
00013 
00014 
00015 SDFileSystem sd(p5, p6, p7, p8, "sd");      //SDcard
00016 SRF02 sensor(p9,p10,0xE0);                  //距離
00017 Servo myservo(p21);                         //モーター
00018 Serial pc(USBTX,USBRX,9600);                //機体teraterm
00019 //Serial xbee(p13,p14,9600);                //xbee(今回使わない)
00020 MPU6050 mpu(p9,p10);                        //９軸
00021 I2C i2c(p28,p27);                           //気圧
00022 LPS ps(i2c);
00023 
00024 int accel[3];
00025 
00026 Timer t;                                    //時間は表示する
00027 
00028 int main() {
00029     t.start();
00030     //xbee.printf("SDcard start\r\n");
00031     pc.printf("SDcard start\r\n");
00032     mkdir("/sd/mydir", 0777);                               //filename
00033     FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");          //open
00034     
00035     if (!ps.init()){
00036         pc.printf("Failed to autodetect pressure sensor!\r\n");
00037         while (1);
00038         }
00039         ps.enableDefault();
00040         
00041         while(1){                                                           //ｄが**cm以下になった場合、for文に移る
00042             myleds[1] = 1; 
00043             wait(0.01);
00044             
00045             //気圧センサー(ここでは表示しない)
00046             //float pressure = ps.readPressureMillibars();
00047             //float altitude = ps.pressureToAltitudeMeters(pressure)-65;     //補正値必須
00048             //float temperature = ps.readTemperatureC();
00049             //pc.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
00050             
00051             //距離SRF02(使う)
00052             int d = sensor.getDistanceCm();
00053             pc.printf("Distance = %d cm\r\n",d);
00054             
00055             //加速度MPU9250(ここでは表示しない)
00056             //mpu.readAccelData(accel);                                          //加速度の値をaccel[3]に代入
00057             //int x = accel[0]-123;                                              //x軸方向の加速度
00058             //int y = accel[1]+60;                                               //y軸方向の加速度
00059             //int z = accel[2]+1110 ;                                            //z軸方向の加速度
00060             //float X = x*0.000597964111328125;
00061             //float Y = y*0.000597964111328125;
00062             //float Z = z*0.000597964111328125;
00063             //double a = X*X+Y*Y+Z*Z-95.982071137936;
00064             //pc.printf("%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);                     //速度と変位を表示
00065             
00066             if(fp == NULL) {                                                    //データが無い場合
00067              error("Could not open file for write\r\n");
00068              }
00069             fprintf(fp, "%d\r\n",d);                                            //距離保存
00070             myleds[1] = 0;
00071             wait(0.01);
00072             
00073             //表示させる文
00074             //pc.printf("t %0.4f p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C ax%.2f ay%.2f az%.2f |a| %.2f %d\r\n",t.read(),pressure,altitude,temperature,X,Y,Z,a,d);
00075             //xbee.printf("number %f t %0.4f p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C ax%.2f ay%.2f az%.2f |a| %.2f\r\n",t.read(),pressure,altitude,temperature,X,Y,Z,a);
00076             
00077             if(d <= 30){
00078              //モータを動かす(スポンジ展開)
00079              for(int motor =0; motor <100; motor++) {
00080                  myservo = motor/100.0;
00081                  wait(0.01);
00082                  }
00083                  for(int num =0;num<1000;num++){                                                                 //1000回行う(コマンドができないため)
00084                     myleds[2] = 1;
00085                     wait(0.01);
00086                     //気圧センサー(表示する)
00087                     float pressure = ps.readPressureMillibars();
00088                     float altitude = ps.pressureToAltitudeMeters(pressure);                                      //補正値必須
00089                     float temperature = ps.readTemperatureC();
00090                     //pc.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
00091                     
00092                     //加速度MPU9250(表示する)
00093                     mpu.readAccelData(accel);                                                                       //加速度の値をaccel[3]に代入
00094                     int x = accel[0]-123;                                                                           //x軸方向の加速度
00095                     int y = accel[1]+60;                                                                            //y軸方向の加速度
00096                     int z = accel[2]+1110 ;                                                                         //z軸方向の加速度
00097                     float X = x*0.000597964111328125;
00098                     float Y = y*0.000597964111328125;
00099                     float Z = z*0.000597964111328125;
00100                     double a = X*X+Y*Y+Z*Z-95.982071137936;
00101                     //pc.printf("%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);
00102                     
00103                     pc.printf("t %0.4f p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C ax%.2f ay%.2f az%.2f |a| %.2f\r\n",t.read(),pressure,altitude,temperature,X,Y,Z,a);
00104                     
00105                     if(fp == NULL) {                                                                                 //データが無い場合
00106                     error("Could not open file for write\r\n");
00107                     }
00108                     fprintf(fp,"%0.4f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",t.read(),pressure,altitude,temperature,X,Y,Z,a);      //SDカードに保存
00109                     //xbee.printf("number %f t %0.4f p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C ax%.2f ay%.2f az%.2f |a| %.2f\r\n",t.read(),pressure,altitude,temperature,X,Y,Z,a);
00110                     myleds[2] = 0;
00111                     wait(0.01);
00112                     }
00113                     
00114                     //回数分行ったら、SDカードを保存終了
00115                     fclose(fp);                                             //保存終了
00116                     //xbee.printf("SDcard OK");
00117                     pc.printf("SDcard OK");
00118                     t.stop();
00119                     }
00120                     }
00121                     }