lpc1768 aassssq
Dependencies: mbed Servo LPS25HB_I2C MPU6050 SDFileSystem
Revision 5:e4fe216f01ad, committed 2022-03-04
- Comitter:
- kosukesuzuki
- Date:
- Fri Mar 04 07:53:32 2022 +0000
- Parent:
- 4:ec40fbfb90f6
- Commit message:
- a
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Feb 28 09:45:43 2022 +0000 +++ b/main.cpp Fri Mar 04 07:53:32 2022 +0000 @@ -1,7 +1,8 @@ //「変更」と書かれている場所の数値は、自分で決めて変える。(主にセンサー数値の補正) +//気圧は基盤のミスのため、今回は使わない #include "mbed.h" #include "MPU6050.h" -#include "LPS.h" +//#include "LPS.h" #include "SDFileSystem.h" #include "Servo.h" @@ -11,16 +12,17 @@ const double Ki =7; //変更 const double Kd =5; //変更 -const double k =0.25; //プロペラの定数(適当に置いたやつ) +//const double k =0.25; //プロペラのための定数(今回使わない) const double mc = 3000; //最大トルク(変更) -const double T = 0; //目標角度°(変更) +const double T1 = 90; //目標角度°(変更) +const double T2 = -90; //目標角度°(変更) DigitalOut led[]={LED1,LED2,LED3,LED4}; -I2C i2c(p28,p27); -LPS ps(i2c); +//I2C i2c(p28,p27); +//LPS ps(i2c); MPU6050 mpu(p9,p10); SDFileSystem sd(p5, p6, p7, p8, "sd"); Serial xbee(p13,p14); @@ -45,6 +47,7 @@ int main() { xbee.printf("motor start\r\n"); led[0]=1; + //モーター準備開始(一時待つ事が必要) myservo1 = myservo2 = myservo3 = myservo4 = 0.0; wait(0.5); @@ -71,14 +74,16 @@ 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"); @@ -93,11 +98,11 @@ //角度算出 mpu.readGyroData(gyro); - int gx = gyro[0]+3656-3505-50-1850+2223-24; //変更 - int gy = gyro[1]-30+710-140-300+375-50-420+255; //変更 - int gz = gyro[2]+5+195-453+300-244-20-10+95+72; //変更 + 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; //変更 @@ -143,7 +148,7 @@ } Tgz = 0; } - + printf("%.2f %.2f %.2f\r\n",GX,GY,GZ); //加速度算出 mpu.readAccelData(accel); @@ -153,65 +158,90 @@ float AX = ax*0.000597964111328125; //変更 float AY = ay*0.000597964111328125; //変更 float AZ = az*0.000597964111328125; //変更 - double ad = AX*AX+AY*AY+AZ*AZ-95.982071137936; - - + 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; - + 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*gY1; - //printf("%.2f %.2f\r\n",c1,c2); //トルク + 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; + double d1,d2,d3,d4; d1 = c1/mc; d2 = c2/mc; - + d3 = c3/mc; + d4 = c4/mc; - // - if(d1 > 0.9){ //変更 - d1 = 0.9; //変更 + //出力の限界 + if(d1 > 0.8){ //変更 + d1 = 0.8; //変更 + } + if(d2 > 0.8){ //変更 + d2 = 0.8; //変更 } - if(d2 > 0.9){ //変更 - d2 = 0.9; //変更 + if(d3 > 0.8){ //変更 + d3 = 0.8; //変更 } - printf("%.2f %.2f\r\n",d1,d2); //duty比 + 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(d1)*k; + myservo2 = abs(d2);//abs(d1)*k;; }else if(GX < 0){ - myservo2 = abs(d1); - myservo1 = abs(d1)*k; + myservo2 = abs(d2); + myservo1 = ; abs(d1); //abs(d1)*k;; //abs(d1)*k; }else{ - myservo1 = myservo2 = 0.025; + myservo1 = myservo2 = ; } if(GY > 0){ - myservo3 = abs(d2); - myservo4 = abs(d2)*k; + myservo3 = abs(d2)+0.025; + myservo4 = 0.025; //abs(d2)*k; //abs(d2)*k; }else if(GY < 0){ - myservo4 = abs(d2); - myservo3 = abs(d2)*k; + 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); @@ -220,14 +250,14 @@ 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); + 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()){ + 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"); @@ -236,7 +266,7 @@ myservo1 = myservo2 = myservo3 = myservo4 = 0.0; led[2]=1; break;} -*/ + } //led[1]=0;