File content as of revision 4:fad5b5a598bf:

//PID制御
#include "mbed.h"
#include "MPU6050.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; //目標角度

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

Servo motor1(p21);
Servo motor2(p22);
Servo motor3(p23);
Servo motor4(p24);

/*
PwmOut led1(LED1); //led
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;
    pc.printf("start\r\n");
    
     
    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);
    
    /*
    //準備
    for (float p=0.0; p<=0.75; p += 0.25) {
            motor1 = motor2 = motor3 = motor4 = p;
            wait(0.5);
        }
    
    wait(1);
    
    motor1 = motor2 = motor3 = motor4 = 0;
    wait(1);
    
    motor1 = motor2 = motor3 = motor4 = 1;
    
    wait(1);
    */
    
    //準備終了
    
    
    
    while(1){
        //角度求める
        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.02562299;//0.025622990.0128114995
        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 > 3){
            if(GX > 0){
                GX = GX - 0.3;
                }else if(GX < 0){
                    GX = GX + 0.3;
                    }else{
                        GX =GX;
                        }
                    Tgx=0;
            }        
        if(Tgy > 3){
            if(GY > 0){
                GY = GY - 0.3;
                }else if(GY < 0){
                    GY = GY + 0.3;
                    }else{
                        GY = GY;
                        }
                    Tgy=0;
            }        
        if(Tgz > 3){
            if(GZ > 0){
                GZ = GZ - 0.3;
                }else if(GZ < 0){
                    GZ = GZ + 0.3;
                    }else{
                        GZ = GZ;
                        }
                    Tgz=0;
            }
        printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);
        
        wait(dt);
        
        //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\r\n",c1);
        
        //回転トルク→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",GX);
        //printf("%.2f %.2f\r\n",d1,d2);
        
        
        //duty比からmotorを動かす。
        if(GX > 0){
            motor1 = abs(d1);
            motor2 = (1-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 = 1-abs(d2);
            }else if(c2 < 0){
                motor4 = abs(d2);
                motor3 = 1-abs(d2);
                }else{
                    motor3 = motor4 = 0;
                    }
        }
    }