File content as of revision 2:c329b0c4ec92:

//PID制御
#include "mbed.h"
#include "MPU6050.h"

const double dt = 0.01; //微小時間

const double Kp =0.3; //変更必須
const double Ki =0.4;
const double Kd =0.5;

const double mc = 77; //最大トルク

const double T  =0; //目標角度

MPU6050 mpu(p9,p10);
Serial pc(USBTX,USBRX);

PwmOut led1(LED1); //motor
PwmOut led2(LED2);
PwmOut led3(LED3);
PwmOut led4(LED4);

int gyro[3];

double GX,GY,GZ;
double Tgx,Tgy,Tgz;

int main() {
    GX = GY = GZ= 0;
    Tgx = Tgy = Tgz = 0;
    
    while(1){
        //角度求める
        mpu.readGyroData(gyro);
        int gx = gyro[0]+3656-3505-50; //変更必須
        int gy = gyro[1]-30;
        int gz = gyro[2]+5;
        printf("%d %d %d\r\n",gx,gy,gz);
        
        double gX = gx*0.02562299;
        double gY = gy*0.02562299;
        double gZ = gz*0.02562299;
        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 > 5){
            if(GX > 0){
                GX = GX - 0.3;
                }else{
                    GX = GX + 0.3;
                    }
                    Tgx=0;
            }        
        if(Tgy > 5){
            if(GY > 0){
                GY = GY - 0.3;
                }else{
                    GY = GY + 0.3;
                    }
                    Tgy=0;
            }        
        if(Tgz > 5){
            if(GZ > 0){
                GZ = GZ - 0.3;
                }else{
                    GZ = GZ + 0.3;
                    }
                    Tgz=0;
            }
        //printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
        
        wait(0.01);
        
        //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 > 1){
            d1 = 1;
            }
        if(d2 > 1){
            d2 = 1;
            }
            
        //duty比からledを動かす。
        if(GX > 0){
            led1 = abs(d1);
            led2 = -abs(d1);
            }else{
                led2 = abs(d1);
                led1 = -abs(d1);
                }
        if(GY > 0){
            led3 = abs(d2);
            led4 = -abs(d2);
            }else{
                led4 = abs(d2);
                led3 = -abs(d2);
                }
        }
    }