lpc1768 aa
Dependencies: mbed Servo LPS25HB_I2C MPU6050 SDFileSystem
main.cpp@2:194e00108f75, 2022-02-17 (annotated)
- Committer:
- kosukesuzuki
- Date:
- Thu Feb 17 15:08:56 2022 +0000
- Revision:
- 2:194e00108f75
- Parent:
- 0:bdbd3d6fc5d5
- Child:
- 3:14b178724982
lpc1768;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbed_official | 0:bdbd3d6fc5d5 | 1 | #include "mbed.h" |
kosukesuzuki | 2:194e00108f75 | 2 | #include "MPU6050.h" |
kosukesuzuki | 2:194e00108f75 | 3 | #include "LPS.h" |
mbed_official | 0:bdbd3d6fc5d5 | 4 | #include "SDFileSystem.h" |
kosukesuzuki | 2:194e00108f75 | 5 | |
kosukesuzuki | 2:194e00108f75 | 6 | I2C i2c(p28,p27); |
kosukesuzuki | 2:194e00108f75 | 7 | LPS ps(i2c); |
kosukesuzuki | 2:194e00108f75 | 8 | MPU6050 mpu(p9,p10); |
kosukesuzuki | 2:194e00108f75 | 9 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
kosukesuzuki | 2:194e00108f75 | 10 | Serial xbee(p13,p14); |
kosukesuzuki | 2:194e00108f75 | 11 | Serial pc(USBTX,USBRX); |
kosukesuzuki | 2:194e00108f75 | 12 | |
kosukesuzuki | 2:194e00108f75 | 13 | int gyro[3]; |
kosukesuzuki | 2:194e00108f75 | 14 | int accel[3]; |
kosukesuzuki | 2:194e00108f75 | 15 | |
kosukesuzuki | 2:194e00108f75 | 16 | double GX,GY,GZ; |
kosukesuzuki | 2:194e00108f75 | 17 | double Tgx,Tgy,Tgz; |
kosukesuzuki | 2:194e00108f75 | 18 | |
mbed_official | 0:bdbd3d6fc5d5 | 19 | int main() { |
kosukesuzuki | 2:194e00108f75 | 20 | xbee.printf("start\r\n"); |
kosukesuzuki | 2:194e00108f75 | 21 | pc.printf("start\r\n"); |
kosukesuzuki | 2:194e00108f75 | 22 | |
kosukesuzuki | 2:194e00108f75 | 23 | GX = 0,GY = 0,GZ = 0; |
kosukesuzuki | 2:194e00108f75 | 24 | Tgx = 0,Tgy = 0,Tgz = 0; |
kosukesuzuki | 2:194e00108f75 | 25 | |
kosukesuzuki | 2:194e00108f75 | 26 | if (!ps.init()){ |
kosukesuzuki | 2:194e00108f75 | 27 | pc.printf("Failed to autodetect pressure sensor!\r\n"); |
kosukesuzuki | 2:194e00108f75 | 28 | while (1); |
kosukesuzuki | 2:194e00108f75 | 29 | } |
kosukesuzuki | 2:194e00108f75 | 30 | |
kosukesuzuki | 2:194e00108f75 | 31 | ps.enableDefault(); |
kosukesuzuki | 2:194e00108f75 | 32 | |
kosukesuzuki | 2:194e00108f75 | 33 | mkdir("/sd/test1", 0777); |
kosukesuzuki | 2:194e00108f75 | 34 | FILE *fp = fopen("/sd/test1/sdtest1.txt", "w"); |
kosukesuzuki | 2:194e00108f75 | 35 | |
kosukesuzuki | 2:194e00108f75 | 36 | fprintf(fp,"start\r\n"); |
mbed_official | 0:bdbd3d6fc5d5 | 37 | |
kosukesuzuki | 2:194e00108f75 | 38 | for(int k = 0; k < 50; k++){ |
kosukesuzuki | 2:194e00108f75 | 39 | |
kosukesuzuki | 2:194e00108f75 | 40 | if(fp == NULL) { |
mbed_official | 0:bdbd3d6fc5d5 | 41 | error("Could not open file for write\n"); |
kosukesuzuki | 2:194e00108f75 | 42 | } |
kosukesuzuki | 2:194e00108f75 | 43 | |
kosukesuzuki | 2:194e00108f75 | 44 | mpu.readGyroData(gyro); |
kosukesuzuki | 2:194e00108f75 | 45 | int gx = gyro[0]+146; |
kosukesuzuki | 2:194e00108f75 | 46 | int gy = gyro[1]+232; //まだ |
kosukesuzuki | 2:194e00108f75 | 47 | int gz = gyro[2]+7; |
kosukesuzuki | 2:194e00108f75 | 48 | //printf("%d %d %d\r\n",gx,gy,gz); |
kosukesuzuki | 2:194e00108f75 | 49 | |
kosukesuzuki | 2:194e00108f75 | 50 | double gX = gx*0.02562; //0.0128114995(測定レンジ±500) |
kosukesuzuki | 2:194e00108f75 | 51 | double gY = gy*0.02562; //0.02562299(±250) |
kosukesuzuki | 2:194e00108f75 | 52 | double gZ = gz*0.02562; |
kosukesuzuki | 2:194e00108f75 | 53 | |
kosukesuzuki | 2:194e00108f75 | 54 | int gX1 = gX; |
kosukesuzuki | 2:194e00108f75 | 55 | int gY1 = gY; |
kosukesuzuki | 2:194e00108f75 | 56 | int gZ1 = gZ; |
kosukesuzuki | 2:194e00108f75 | 57 | |
kosukesuzuki | 2:194e00108f75 | 58 | double gX2 = gX1*0.01; |
kosukesuzuki | 2:194e00108f75 | 59 | double gY2 = gY1*0.01; |
kosukesuzuki | 2:194e00108f75 | 60 | double gZ2 = gZ1*0.01; |
kosukesuzuki | 2:194e00108f75 | 61 | |
kosukesuzuki | 2:194e00108f75 | 62 | GX = GX + gX2; |
kosukesuzuki | 2:194e00108f75 | 63 | GY = GY + gY2; |
kosukesuzuki | 2:194e00108f75 | 64 | GZ = GZ + gZ2; |
kosukesuzuki | 2:194e00108f75 | 65 | |
kosukesuzuki | 2:194e00108f75 | 66 | Tgx = Tgx + abs(gX2); |
kosukesuzuki | 2:194e00108f75 | 67 | Tgy = Tgy + abs(gY2); |
kosukesuzuki | 2:194e00108f75 | 68 | Tgz = Tgz + abs(gZ2); |
kosukesuzuki | 2:194e00108f75 | 69 | |
kosukesuzuki | 2:194e00108f75 | 70 | if(Tgx > 10){ |
kosukesuzuki | 2:194e00108f75 | 71 | if(GX > 0){ |
kosukesuzuki | 2:194e00108f75 | 72 | GX = GX - 0.3;//変更必須 |
kosukesuzuki | 2:194e00108f75 | 73 | }else{ |
kosukesuzuki | 2:194e00108f75 | 74 | GX = GX + 0.3; |
kosukesuzuki | 2:194e00108f75 | 75 | } |
kosukesuzuki | 2:194e00108f75 | 76 | Tgx = 0; |
kosukesuzuki | 2:194e00108f75 | 77 | } |
kosukesuzuki | 2:194e00108f75 | 78 | if(Tgy > 10){ |
kosukesuzuki | 2:194e00108f75 | 79 | if(GY >0){ |
kosukesuzuki | 2:194e00108f75 | 80 | GY = GY - 0.3; |
kosukesuzuki | 2:194e00108f75 | 81 | }else{ |
kosukesuzuki | 2:194e00108f75 | 82 | GY = GY + 0.3; |
kosukesuzuki | 2:194e00108f75 | 83 | } |
kosukesuzuki | 2:194e00108f75 | 84 | Tgy = 0; |
kosukesuzuki | 2:194e00108f75 | 85 | } |
kosukesuzuki | 2:194e00108f75 | 86 | if(Tgz > 10){ |
kosukesuzuki | 2:194e00108f75 | 87 | if(GZ >0){ |
kosukesuzuki | 2:194e00108f75 | 88 | GZ = GZ - 0.3; |
kosukesuzuki | 2:194e00108f75 | 89 | }else{ |
kosukesuzuki | 2:194e00108f75 | 90 | GZ = GZ + 0.3; |
kosukesuzuki | 2:194e00108f75 | 91 | } |
kosukesuzuki | 2:194e00108f75 | 92 | Tgz = 0; |
kosukesuzuki | 2:194e00108f75 | 93 | } |
kosukesuzuki | 2:194e00108f75 | 94 | |
kosukesuzuki | 2:194e00108f75 | 95 | mpu.readAccelData(accel); |
kosukesuzuki | 2:194e00108f75 | 96 | int ax = accel[0]-123;//x軸方向の加速度 |
kosukesuzuki | 2:194e00108f75 | 97 | int ay = accel[1]+60;//y軸方向の加速度 |
kosukesuzuki | 2:194e00108f75 | 98 | int az = accel[2]+1110 ;//z軸方向の加速度 |
kosukesuzuki | 2:194e00108f75 | 99 | float AX = ax*0.000597964111328125; |
kosukesuzuki | 2:194e00108f75 | 100 | float AY = ay*0.000597964111328125; |
kosukesuzuki | 2:194e00108f75 | 101 | float AZ = az*0.000597964111328125; |
kosukesuzuki | 2:194e00108f75 | 102 | double a = AX*AX+AY*AY+AZ*AZ-95.982071137936; |
kosukesuzuki | 2:194e00108f75 | 103 | |
kosukesuzuki | 2:194e00108f75 | 104 | float pres = ps.readPressureMillibars(); |
kosukesuzuki | 2:194e00108f75 | 105 | float altit = ps.pressureToAltitudeMeters(pres); |
kosukesuzuki | 2:194e00108f75 | 106 | float tempe = ps.readTemperatureC(); |
kosukesuzuki | 2:194e00108f75 | 107 | |
kosukesuzuki | 2:194e00108f75 | 108 | pc.printf("%.2f %.2f %.2f %.2f\r\n",AX,AY,AZ,a); |
kosukesuzuki | 2:194e00108f75 | 109 | pc.printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); |
kosukesuzuki | 2:194e00108f75 | 110 | pc.printf("%.2f %.2f %.2f\r\n",pres,altit,tempe); |
kosukesuzuki | 2:194e00108f75 | 111 | |
kosukesuzuki | 2:194e00108f75 | 112 | xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe); |
kosukesuzuki | 2:194e00108f75 | 113 | fprintf(fp, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe); |
kosukesuzuki | 2:194e00108f75 | 114 | wait(0.01); |
kosukesuzuki | 2:194e00108f75 | 115 | } |
kosukesuzuki | 2:194e00108f75 | 116 | |
kosukesuzuki | 2:194e00108f75 | 117 | fprintf(fp,"end\r\n"); |
kosukesuzuki | 2:194e00108f75 | 118 | fclose(fp); |
kosukesuzuki | 2:194e00108f75 | 119 | xbee.printf("end\r\n"); |
kosukesuzuki | 2:194e00108f75 | 120 | pc.printf("end\r\n"); |
kosukesuzuki | 2:194e00108f75 | 121 | |
kosukesuzuki | 2:194e00108f75 | 122 | return 0; |
kosukesuzuki | 2:194e00108f75 | 123 | } |