lpc1768 aassssq

Dependencies:   mbed Servo LPS25HB_I2C MPU6050 SDFileSystem

main.cpp

Committer:
kosukesuzuki
Date:
2022-02-17
Revision:
2:194e00108f75
Parent:
0:bdbd3d6fc5d5
Child:
3:14b178724982

File content as of revision 2:194e00108f75:

#include "mbed.h"
#include "MPU6050.h"
#include "LPS.h"
#include "SDFileSystem.h"

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() {
    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");
    
    for(int k = 0; k < 50; k++){
         
         if(fp == NULL) {
        error("Could not open file for write\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;
}