lpc1768 aassssq
Dependencies: mbed Servo LPS25HB_I2C MPU6050 SDFileSystem
main.cpp
- Committer:
- kosukesuzuki
- Date:
- 2022-02-25
- Revision:
- 3:14b178724982
- Parent:
- 2:194e00108f75
- Child:
- 4:ec40fbfb90f6
File content as of revision 3:14b178724982:
//「変更」と書かれている場所の数値は、自分で決めて変える。(主にセンサー数値の補正) #include "mbed.h" #include "MPU6050.h" #include "LPS.h" #include "SDFileSystem.h" #include "Servo.h" const double dt = 0.01; //微小時間(変更) const double Kp =5; //変更 const double Ki =3; //変更 const double Kd =3; //変更 const double mc = 1995; //最大トルク(変更) const double T =0; //目標角度°(変更) 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); Servo motor1(p21); Servo motor2(p22); Servo motor3(p23); Servo motor4(p24); Timer ti; int cmd = xbee.getc(); int gyro[3]; int accel[3]; int count; double GX,GY,GZ; double Tgx,Tgy,Tgz; int main() { //モーター準備開始(一時待つ事が必要) motor1 = motor2 = motor3 = motor4 = 0.0; wait(0.5); motor1 = motor2 = motor3 = motor4 = 1.0; wait(8); motor1 = motor2 = motor3 = motor4 = 0.0; wait(8); //モーター準備終了 //pcのxbeeからのコマンド「c」で開始 if(cmd = 'c'){ xbee.printf("start\r\n"); pc.printf("start\r\n"); count = 0; 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"); ti.start(); while(1){ if(fp == NULL) { error("Could not open file for write\n"); } //角度算出 mpu.readGyroData(gyro); int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更 int gy = gyro[1]-30+710-140-300+375-50; //変更 int gz = gyro[2]+5+195-453+300-244-20-10; //変更 //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*dt; double gY2 = gY1*dt; double gZ2 = gZ1*dt; GX = GX + gX2; GY = GY + gY2; GZ = GZ + gZ2; Tgx = Tgx + abs(gX2); Tgy = Tgy + abs(gY2); Tgz = Tgz + abs(gZ2); //和の補正(精度はよくない) if(Tgx > 3){ if(GX > 0){ GX = GX - 0.3;//変更 }else{ GX = GX + 0.3;//変更 } Tgx = 0; } if(Tgy > 3){ if(GY >0){ GY = GY - 0.3;//変更 }else{ GY = GY + 0.3;//変更 } Tgy = 0; } if(Tgz > 3){ if(GZ >0){ GZ = GZ - 0.3;//変更 }else{ GZ = GZ + 0.3;//変更 } Tgz = 0; } //加速度算出 mpu.readAccelData(accel); int ax = accel[0]-123; //変更 int ay = accel[1]+60; //変更 int az = accel[2]+1110 ; //変更 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(); //PID開始 //必要なトルクを算出 double c1,c2; double f1,f2; f1 = T - GX; f2 = T - GY; c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク c2 = Kp*f2+Ki*f2*dt+Kd*gY1; //printf("%.2f %.2f\r\n",c1,c2); //トルク //回転トルク→duty比を算出 double d1,d2; d1 = c1/mc; d2 = c2/mc; if(d1 > 0.9){ //変更 d1 = 0.9; //変更 } if(d2 > 0.9){ //変更 d2 = 0.9; //変更 } //printf("%.2f %.2f\r\n",d1,d2); //duty比 //試行回数 count = count + 1; //duty比からmotorを動かす。 if(GX > 0){ motor1 = abs(d1); motor2 = 0.5-abs(d1)/2; //変更 }else if(c1 < 0){ motor2 = abs(d1); motor1 = 0.5-abs(d1)/2; //変更 }else{ motor1 = motor2 = 0; } if(GX > 0){ motor3 = abs(d2); motor4 = 0.5-abs(d2)/2; //変更 }else if(c2 < 0){ motor4 = abs(d2); motor3 = 0.5-abs(d2)/2; //変更 }else{ motor3 = motor4 = 0; } /*主なセンサー表示 pc.printf("T %.2f\r\n",ti.read()); pc.printf("c %d\r\n",count); pc.printf("ax%.2f ay%.2f az%.2f |a|%.2f\r\n",AX,AY,AZ,a); pc.printf("gx%.2f gy%.2f gz%.2f\r\n",GX,GY,GZ); pc.printf("p%.2f alt%.2f t%.2f\r\n",pres,altit,tempe); */ pc.printf("c%d, T%.2f, ax%.2f,ay%.2f,az%.2f,|a|%.2f, gx%.2f,gy%.2f,gz%.2f, p%.2f,al%.2f,t%.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,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, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,pres,altit,tempe,c1,c2,d1,d2); //fprintf(fp,"%.2f %.2f %.2f %.2f %.2f\r\n",ti.read(),c1,c2,d1,d2); //調整用 wait(dt); if(AZ > 9 && AZ < 11 && a > 8 && a < 11 && 3 < ti.read()){ fprintf(fp,"end\r\n"); fclose(fp); //注意:ここまで行わないと、SDカードに保存されない xbee.printf("end\r\n"); pc.printf("end\r\n"); ti.stop(); return 0; } } } }