File content as of revision 1:50963ee12158:

#include "mbed.h"
#include "MPU9250.h"
#include "math.h"
#include "Servo.h"
#define PI 3.14159265

SPI spi(PA_7, PA_6, PA_5);
mpu9250_spi imu(spi,PB_6);
Serial pc(SERIAL_TX, SERIAL_RX); 

/*SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
mpu9250_spi imu(spi,SPI_CS);*/
Servo servo1(D8);
Servo servo2(D9);

Timer clk;
float meas1,meas2,meas3;
double Axr,Ayr,Azr;
float Xdeg,Ydeg,Zdeg,Ydeg2;
float Tin;

float gyro_x_cal = 0;
float gyro_y_cal = 0;
float gyro_z_cal = 0;

float alpha,alpha_dot;
float omg;
float alpha_old = 0;
float alpha_new = 0;
double theta;

float angle1;
float angle_dot;
float u;
float u2;
float kp;
float kd;
float pwm1;
float pwm2;
//======kalman filter====
float Angle=0.0; 
float Q_angle=0.9;  
float Q_gyro=0.001;
float R_angle=0.5;
char  C_0 = 1;
float dt=0.01;
float Q_bias, Angle_err;
float PCt_0=0.0, PCt_1=0.0, E=0.0;
float K_0=0.0, K_1=0.0, t_0=0.0, t_1=0.0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
float Accel;
float Gyro;
float Gyro_y ;
//=======================
float num;
char kb='p';
float i=0.1;
int main() {
    pc.baud(115200); //設定串列通訊包率
    //==========MPU9250 Setting==========
    printf("\nAnalogIn example\n");
    if(imu.init(1,BITS_DLPF_CFG_188HZ)){  //INIT the mpu9250
        pc.printf("\nCouldn't initialize MPU9250 via SPI!");
    }    
    pc.printf("WHOAMI=0x%2x\n",imu.whoami()); //
    pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
    pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
    pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
    //===================================
    
    printf("Press any key to START\n");
while (kb=='p') {
        scanf("%c",&kb);
    }
    servo1.calibrate(0.02, 0.02*0.065, 0.02*0.085);
    servo2.calibrate(0.02, 0.02*0.065, 0.02*0.085);
    
    for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
         imu.read_all();
         gyro_x_cal += imu.gyroscope_data[0];   //Add the gyro x offset to the gyro_x_cal variable
         gyro_y_cal += imu.gyroscope_data[1];   //Add the gyro y offset to the gyro_y_cal variable
         gyro_z_cal += imu.gyroscope_data[2];   //Add the gyro z offset to the gyro_z_cal variable
        }
        // divide by 1000 to get avarage offset
    gyro_x_cal /= 1000;
    gyro_y_cal /= 1000;
    gyro_z_cal /= 1000;
 
    clk.start();
    Tin = clk.read(); 
    
    while(1) {
        imu.read_all(); //每次要讀取都要有這一項來宣告
        //lib中都已經事先除"轉換倍率"了
        meas1 = imu.gyroscope_data[0] - gyro_x_cal; // Converts and read the analog input value (value from 0.0 to 1.0
        meas2 = imu.gyroscope_data[1]- gyro_y_cal; 
        meas3 = imu.gyroscope_data[2] - gyro_z_cal;
        
        Xdeg = meas1*0.01*PI/180;//輸出為角度轉徑度
        Ydeg += meas2*0.01;//*PI/180;//0.002 = dt
        Zdeg = meas3*0.01*PI/180;
        Ydeg2=Ydeg;
        
        
        Axr = imu.accelerometer_data[0]; // Converts and read the analog input value (value from 0.0 to 1.0
        Ayr = imu.accelerometer_data[1]; //原始輸出都是以重力加速度g來看
        Azr = imu.accelerometer_data[2]; // 
        
        theta = -atan2(Axr,Azr)*180/PI;//
        
        //==========kalman filter================
        Accel = theta;
        Gyro= meas2 ;
        
        Angle+=(Gyro - Q_bias) * dt; 
        
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; 
 
        Pdot[1]=- PP[1][1];
        Pdot[2]=- PP[1][1];
        Pdot[3]=Q_gyro;
         
        PP[0][0] += Pdot[0] * dt;   
        PP[0][1] += Pdot[1] * dt;   
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
                 
        Angle_err = Accel - Angle;
         
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
         
        E = R_angle + C_0 * PCt_0;
         
        if(E!=0)
        {
          K_0 = PCt_0 / E;
          K_1 = PCt_1 / E;
        }
         
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];
 
        PP[0][0] -= K_0 * t_0;
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
                 
        Angle        += K_0 * Angle_err;
        Q_bias        += K_1 * Angle_err;
        Gyro_y   = Gyro - Q_bias;
        //===========Alpha dot=============
        //alpha_dot = meas2*PI/180;//因為此處是設定以y軸當作轉軸來看(轉成徑度)
        kp=0.5;
        kd=0.2;
        angle1=Angle*PI/180;
        angle_dot=meas2*PI/180;
        u=kp*abs(angle1)+kd*abs(angle_dot);
        //pwm1=0.5+u;
        //pwm2=0.5-u;
        //u2=u*0.07*sin(angle1);
        
        if (Angle>4)
        {
         pwm1=0.5+u;
         pwm2=0.5-u;    
        }
        
        else if (Angle<-4)
        {
         pwm1=0.5-u;
         pwm2=0.5+u;   
        }
        else if (0<Angle<4 || 0>Angle>-4)
        {
         pwm1=0.5;
         pwm2=0.5;   
        }
     
        
        servo1.write( pwm1);
        servo2.write( pwm2);
        
        while((clk.read() - Tin)<0.01){} //設定取樣時間
        Tin = clk.read();   //Reset the loop timer
        
        
        
        printf("angle_dot=%.3f  ,Angle=%.3f\n",meas2,Angle);
        printf("u=%.3f\n",u);
        
          }
}