Diff: main.cpp

Revision:

2:194e00108f75

Parent:

0:bdbd3d6fc5d5

Child:

3:14b178724982

--- a/main.cpp	Tue May 16 05:18:55 2017 +0000
+++ b/main.cpp	Thu Feb 17 15:08:56 2022 +0000
@@ -1,19 +1,123 @@
 #include "mbed.h"
+#include "MPU6050.h"
+#include "LPS.h"
 #include "SDFileSystem.h"
- 
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
- 
+
+I2C i2c(p28,p27);
+LPS ps(i2c);
+MPU6050 mpu(p9,p10);
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+Serial xbee(p13,p14);
+Serial pc(USBTX,USBRX);
+
+int gyro[3];
+int accel[3];
+
+double GX,GY,GZ;
+double Tgx,Tgy,Tgz;
+
 int main() {
-    printf("Hello World!\n");   
- 
-    mkdir("/sd/mydir", 0777);
+    xbee.printf("start\r\n");
+    pc.printf("start\r\n");
+    
+    GX = 0,GY = 0,GZ = 0;
+    Tgx = 0,Tgy = 0,Tgz = 0;
+    
+    if (!ps.init()){
+        pc.printf("Failed to autodetect pressure sensor!\r\n");
+        while (1);
+    }
+    
+    ps.enableDefault();
+    
+    mkdir("/sd/test1", 0777);
+    FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
+    
+    fprintf(fp,"start\r\n");
     
-    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    if(fp == NULL) {
+    for(int k = 0; k < 50; k++){
+         
+         if(fp == NULL) {
         error("Could not open file for write\n");
-    }
-    fprintf(fp, "Hello fun SD Card World!");
-    fclose(fp); 
- 
-    printf("Goodbye World!\n");
-}
+        }
+        
+         mpu.readGyroData(gyro);
+         int gx = gyro[0]+146;
+         int gy = gyro[1]+232;  //まだ
+         int gz = gyro[2]+7;
+         //printf("%d %d %d\r\n",gx,gy,gz);
+         
+         double gX = gx*0.02562; //0.0128114995(測定レンジ±500)
+         double gY = gy*0.02562; //0.02562299(±250)
+         double gZ = gz*0.02562;
+         
+         int gX1 = gX;
+         int gY1 = gY;
+         int gZ1 = gZ;
+         
+         double gX2 = gX1*0.01;
+         double gY2 = gY1*0.01;
+         double gZ2 = gZ1*0.01;
+         
+         GX = GX + gX2;
+         GY = GY + gY2;
+         GZ = GZ + gZ2;
+         
+         Tgx = Tgx + abs(gX2);
+         Tgy = Tgy + abs(gY2);
+         Tgz = Tgz + abs(gZ2);
+         
+         if(Tgx > 10){
+             if(GX > 0){
+                GX = GX - 0.3;//変更必須
+                }else{
+                    GX = GX + 0.3;
+                }
+             Tgx = 0;
+             }
+         if(Tgy > 10){
+             if(GY >0){
+                 GY = GY - 0.3;
+                }else{
+                    GY = GY + 0.3;
+                } 
+             Tgy = 0;
+             }
+         if(Tgz > 10){
+             if(GZ >0){
+                 GZ = GZ - 0.3;
+                 }else{
+                     GZ = GZ + 0.3;
+                     }
+             Tgz = 0;
+             }
+             
+        mpu.readAccelData(accel);
+        int ax = accel[0]-123;//x軸方向の加速度
+        int ay = accel[1]+60;//y軸方向の加速度
+        int az = accel[2]+1110 ;//z軸方向の加速度
+        float AX = ax*0.000597964111328125;
+        float AY = ay*0.000597964111328125;
+        float AZ = az*0.000597964111328125; 
+        double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
+        
+        float pres = ps.readPressureMillibars();
+        float altit = ps.pressureToAltitudeMeters(pres);
+        float tempe = ps.readTemperatureC();
+        
+        pc.printf("%.2f %.2f %.2f %.2f\r\n",AX,AY,AZ,a);
+        pc.printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
+        pc.printf("%.2f %.2f %.2f\r\n",pres,altit,tempe);
+        
+        xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
+        fprintf(fp, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe);
+        wait(0.01);
+        }
+        
+        fprintf(fp,"end\r\n");
+        fclose(fp);
+        xbee.printf("end\r\n");
+        pc.printf("end\r\n");
+        
+        return 0;
+}
\ No newline at end of file