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; }