lpc1768 aassssq
Dependencies: mbed Servo LPS25HB_I2C MPU6050 SDFileSystem
main.cpp
- Committer:
- kosukesuzuki
- Date:
- 2022-03-04
- Revision:
- 5:e4fe216f01ad
- Parent:
- 4:ec40fbfb90f6
File content as of revision 5:e4fe216f01ad:
//「変更」と書かれている場所の数値は、自分で決めて変える。(主にセンサー数値の補正) //気圧は基盤のミスのため、今回は使わない #include "mbed.h" #include "MPU6050.h" //#include "LPS.h" #include "SDFileSystem.h" #include "Servo.h" const double dt = 0.01; //微小時間(変更) const double Kp =11; //変更 const double Ki =7; //変更 const double Kd =5; //変更 //const double k =0.25; //プロペラのための定数(今回使わない) const double mc = 3000; //最大トルク(変更) const double T1 = 90; //目標角度°(変更) const double T2 = -90; //目標角度°(変更) DigitalOut led[]={LED1,LED2,LED3,LED4}; //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 myservo1(p21); Servo myservo2(p22); Servo myservo3(p23); Servo myservo4(p24); Timer ti; int gyro[3]; int accel[3]; int count; double GX,GY,GZ; double Tgx,Tgy,Tgz; int cmd; int main() { xbee.printf("motor start\r\n"); led[0]=1; //モーター準備開始(一時待つ事が必要) myservo1 = myservo2 = myservo3 = myservo4 = 0.0; wait(0.5); myservo1 = myservo2 = myservo3 = myservo4 = 1.0; wait(8); myservo1 = myservo2 = myservo3 = myservo4 = 0.0; wait(8); xbee.printf("motor end\r\n"); //モーター準備終了 led[1]=1; //約5秒で開始 wait(5); wait(1.0); led[0]=1; wait(1.0); led[0]=0; led[1]=0; 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]+420; //変更 int gy = gyro[1]-1424+185+26-34+53; //変更 int gz = gyro[2]; //変更 //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; } printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); //加速度算出 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,c3,c4; double f1,f2,f3,f4; //GX f1 = T1 - GX; f2 = T2 - GX; //GY f3 = T1 - GY; f4 = T2 - GY; //GX c1 = Kp*f1+Ki*f1*dt-Kd*gX1; //トルク c2 = Kp*f2+Ki*f2*dt-Kd*gX1; //GY c3 = Kp*f3+Ki*f3*dt-Kd*gY1; c4 = Kp*f4+Ki*f4*dt-Kd*gY1; //printf("%.2f %.2f %.2f %.2f\r\n",c1,c2,c3,c4); //トルク //回転トルク→duty比を算出 double d1,d2,d3,d4; d1 = c1/mc; d2 = c2/mc; d3 = c3/mc; d4 = c4/mc; //出力の限界 if(d1 > 0.8){ //変更 d1 = 0.8; //変更 } if(d2 > 0.8){ //変更 d2 = 0.8; //変更 } if(d3 > 0.8){ //変更 d3 = 0.8; //変更 } if(d4 > 0.8){ //変更 d4 = 0.8; //変更 } //printf("%.2f %.2f %.2f %.2f\r\n",d1,d2,d3,d4); //duty比 //試行回数 count = count + 1; //duty比からmotorを動かす。 myservo1 = abs(d1); myservo2 = abs(d2); myservo3 = abs(d3); myservo4 = abs(d4); /* duty比が2つだけのとき(定数kを使用) if(GX > 0){ myservo1 = abs(d1); myservo2 = abs(d2);//abs(d1)*k;; }else if(GX < 0){ myservo2 = abs(d2); myservo1 = ; abs(d1); //abs(d1)*k;; //abs(d1)*k; }else{ myservo1 = myservo2 = ; } if(GY > 0){ myservo3 = abs(d2)+0.025; myservo4 = 0.025; //abs(d2)*k; //abs(d2)*k; }else if(GY < 0){ myservo4 = abs(d2)+0.025; myservo3 = 0.025; //abs(d2)*k; }else{ myservo3 = myservo4 = 0.025; } */ /*主なセンサー表示 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\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ); xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ); fprintf(fp, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,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 && 5 < ti.read()){ fprintf(fp,"end\r\n"); fclose(fp); //注意:ここまで行わないと、SDカードに保存されない xbee.printf("end\r\n"); pc.printf("end\r\n"); ti.stop(); myservo1 = myservo2 = myservo3 = myservo4 = 0.0; led[2]=1; break;} } //led[1]=0; //led[3]=1; }