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比が２つだけのとき(定数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;
            
        }