main.cpp

00001 //PID制御
00002 #include "mbed.h"
00003 #include "MPU6050.h"
00004 
00005 const double dt = 0.01; //微小時間
00006 
00007 const double Kp =0.3; //変更必須
00008 const double Ki =0.4;
00009 const double Kd =0.5;
00010 
00011 const double mc = 77; //最大トルク
00012 
00013 const double T  =0; //目標角度
00014 
00015 MPU6050 mpu(p9,p10);
00016 Serial pc(USBTX,USBRX);
00017 
00018 PwmOut led1(LED1); //motor
00019 PwmOut led2(LED2);
00020 PwmOut led3(LED3);
00021 PwmOut led4(LED4);
00022 
00023 int gyro[3];
00024 
00025 double GX,GY,GZ;
00026 double Tgx,Tgy,Tgz;
00027 
00028 int main() {
00029     GX = GY = GZ= 0;
00030     Tgx = Tgy = Tgz = 0;
00031     
00032     while(1){
00033         //角度求める
00034         mpu.readGyroData(gyro);
00035         int gx = gyro[0]+3656-3505-50; //変更必須
00036         int gy = gyro[1]-30;
00037         int gz = gyro[2]+5;
00038         printf("%d %d %d\r\n",gx,gy,gz);
00039         
00040         double gX = gx*0.02562299;
00041         double gY = gy*0.02562299;
00042         double gZ = gz*0.02562299;
00043         int gX1 = gX;
00044         int gY1 = gY;
00045         int gZ1 = gZ;
00046         double gX2 = gX1*dt;
00047         double gY2 = gY1*dt;
00048         double gZ2 = gZ1*dt;
00049         
00050         GX = GX + gX2;
00051         GY = GY + gY2;
00052         GZ = GZ + gZ2;
00053         
00054         Tgx = Tgx + abs(gX2);
00055         Tgy = Tgy + abs(gY2);
00056         Tgz = Tgz + abs(gZ2);
00057         
00058         
00059         if(Tgx > 5){
00060             if(GX > 0){
00061                 GX = GX - 0.3;
00062                 }else{
00063                     GX = GX + 0.3;
00064                     }
00065                     Tgx=0;
00066             }        
00067         if(Tgy > 5){
00068             if(GY > 0){
00069                 GY = GY - 0.3;
00070                 }else{
00071                     GY = GY + 0.3;
00072                     }
00073                     Tgy=0;
00074             }        
00075         if(Tgz > 5){
00076             if(GZ > 0){
00077                 GZ = GZ - 0.3;
00078                 }else{
00079                     GZ = GZ + 0.3;
00080                     }
00081                     Tgz=0;
00082             }
00083         //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
00084         
00085         wait(0.01);
00086         
00087         //PID()
00088         double c1,c2;
00089         double f1,f2;
00090         f1 = T - GX;
00091         f2 = T - GY;
00092 
00093         c1 = Kp*f1+Ki*f1*dt+Kd*gX1; //トルク
00094         c2 = Kp+f2+Ki*f2*dt+Kd*gY1;
00095         //printf("%.2f %.2f\r\n",c1,c2);
00096         
00097         //回転トルク→duty比
00098         double d1,d2;
00099         d1 = c1/mc;
00100         d2 = c2/mc;
00101         
00102         if(d1 > 1){
00103             d1 = 1;
00104             }
00105         if(d2 > 1){
00106             d2 = 1;
00107             }
00108             
00109         //duty比からledを動かす。
00110         if(GX > 0){
00111             led1 = abs(d1);
00112             }else{
00113                 led2 = abs(d1);
00114                 }
00115         if(GY > 0){
00116             led3 = abs(d2);
00117             }else{
00118                 led4 = abs(d2);
00119                 }
00120         }
00121     }