self_balance
Dependencies: MPU9250_SPI Servo mbed
main.cpp
- Committer:
- billwu1149
- Date:
- 2017-07-11
- Revision:
- 0:810f12542b58
- Child:
- 1:50963ee12158
File content as of revision 0:810f12542b58:
#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.35; kd=0.03; 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 (0<Angle<3 || 0>Angle>-3) { servo1.write(0.5); servo2.write(0.5); } if (Angle>3) { servo1.write( pwm1); servo2.write( pwm2); } if (Angle<-3) { servo1.write( pwm2); servo2.write( pwm1); } while((clk.read() - Tin)<0.01){} //設定取樣時間 Tin = clk.read(); //Reset the loop timer printf("angle_dot=%f ,Angle=%f\n",meas2,Angle); printf("u=%f\n",u); } }