wu bill / Mbed 2 deprecated self_balance

self_balance

Dependencies:   MPU9250_SPI Servo mbed

main.cpp@1:50963ee12158, 2018-03-25 (annotated)

Committer:
billwu1149
Date:
Sun Mar 25 06:35:59 2018 +0000
Revision:
1:50963ee12158
Parent:
0:810f12542b58 
aecl

Who changed what in which revision?

UserRevisionLine numberNew contents of line
billwu1149 0:810f12542b58 1 #include "mbed.h"
billwu1149 0:810f12542b58 2 #include "MPU9250.h"
billwu1149 0:810f12542b58 3 #include "math.h"
billwu1149 0:810f12542b58 4 #include "Servo.h"
billwu1149 0:810f12542b58 5 #define PI 3.14159265
billwu1149 0:810f12542b58 6
billwu1149 0:810f12542b58 7 SPI spi(PA_7, PA_6, PA_5);
billwu1149 0:810f12542b58 8 mpu9250_spi imu(spi,PB_6);
billwu1149 0:810f12542b58 9 Serial pc(SERIAL_TX, SERIAL_RX);
billwu1149 0:810f12542b58 10
billwu1149 0:810f12542b58 11 /*SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
billwu1149 0:810f12542b58 12 mpu9250_spi imu(spi,SPI_CS);*/
billwu1149 0:810f12542b58 13 Servo servo1(D8);
billwu1149 0:810f12542b58 14 Servo servo2(D9);
billwu1149 0:810f12542b58 15
billwu1149 0:810f12542b58 16 Timer clk;
billwu1149 0:810f12542b58 17 float meas1,meas2,meas3;
billwu1149 0:810f12542b58 18 double Axr,Ayr,Azr;
billwu1149 0:810f12542b58 19 float Xdeg,Ydeg,Zdeg,Ydeg2;
billwu1149 0:810f12542b58 20 float Tin;
billwu1149 0:810f12542b58 21
billwu1149 0:810f12542b58 22 float gyro_x_cal = 0;
billwu1149 0:810f12542b58 23 float gyro_y_cal = 0;
billwu1149 0:810f12542b58 24 float gyro_z_cal = 0;
billwu1149 0:810f12542b58 25
billwu1149 0:810f12542b58 26 float alpha,alpha_dot;
billwu1149 0:810f12542b58 27 float omg;
billwu1149 0:810f12542b58 28 float alpha_old = 0;
billwu1149 0:810f12542b58 29 float alpha_new = 0;
billwu1149 0:810f12542b58 30 double theta;
billwu1149 0:810f12542b58 31
billwu1149 0:810f12542b58 32 float angle1;
billwu1149 0:810f12542b58 33 float angle_dot;
billwu1149 0:810f12542b58 34 float u;
billwu1149 0:810f12542b58 35 float u2;
billwu1149 0:810f12542b58 36 float kp;
billwu1149 0:810f12542b58 37 float kd;
billwu1149 0:810f12542b58 38 float pwm1;
billwu1149 0:810f12542b58 39 float pwm2;
billwu1149 0:810f12542b58 40 //======kalman filter====
billwu1149 0:810f12542b58 41 float Angle=0.0;
billwu1149 0:810f12542b58 42 float Q_angle=0.9;
billwu1149 0:810f12542b58 43 float Q_gyro=0.001;
billwu1149 0:810f12542b58 44 float R_angle=0.5;
billwu1149 0:810f12542b58 45 char C_0 = 1;
billwu1149 0:810f12542b58 46 float dt=0.01;
billwu1149 0:810f12542b58 47 float Q_bias, Angle_err;
billwu1149 0:810f12542b58 48 float PCt_0=0.0, PCt_1=0.0, E=0.0;
billwu1149 0:810f12542b58 49 float K_0=0.0, K_1=0.0, t_0=0.0, t_1=0.0;
billwu1149 0:810f12542b58 50 float Pdot[4] ={0,0,0,0};
billwu1149 0:810f12542b58 51 float PP[2][2] = { { 1, 0 },{ 0, 1 } };
billwu1149 0:810f12542b58 52 float Accel;
billwu1149 0:810f12542b58 53 float Gyro;
billwu1149 0:810f12542b58 54 float Gyro_y ;
billwu1149 0:810f12542b58 55 //=======================
billwu1149 0:810f12542b58 56 float num;
billwu1149 0:810f12542b58 57 char kb='p';
billwu1149 0:810f12542b58 58 float i=0.1;
billwu1149 0:810f12542b58 59 int main() {
billwu1149 0:810f12542b58 60 pc.baud(115200); //設定串列通訊包率
billwu1149 0:810f12542b58 61 //==========MPU9250 Setting==========
billwu1149 0:810f12542b58 62 printf("\nAnalogIn example\n");
billwu1149 0:810f12542b58 63 if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250
billwu1149 0:810f12542b58 64 pc.printf("\nCouldn't initialize MPU9250 via SPI!");
billwu1149 0:810f12542b58 65 }
billwu1149 0:810f12542b58 66 pc.printf("WHOAMI=0x%2x\n",imu.whoami()); //
billwu1149 0:810f12542b58 67 pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
billwu1149 0:810f12542b58 68 pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
billwu1149 0:810f12542b58 69 pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
billwu1149 0:810f12542b58 70 //===================================
billwu1149 0:810f12542b58 71
billwu1149 0:810f12542b58 72 printf("Press any key to START\n");
billwu1149 0:810f12542b58 73 while (kb=='p') {
billwu1149 0:810f12542b58 74 scanf("%c",&kb);
billwu1149 0:810f12542b58 75 }
billwu1149 0:810f12542b58 76 servo1.calibrate(0.02, 0.02*0.065, 0.02*0.085);
billwu1149 0:810f12542b58 77 servo2.calibrate(0.02, 0.02*0.065, 0.02*0.085);
billwu1149 0:810f12542b58 78
billwu1149 0:810f12542b58 79 for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
billwu1149 0:810f12542b58 80 imu.read_all();
billwu1149 0:810f12542b58 81 gyro_x_cal += imu.gyroscope_data[0]; //Add the gyro x offset to the gyro_x_cal variable
billwu1149 0:810f12542b58 82 gyro_y_cal += imu.gyroscope_data[1]; //Add the gyro y offset to the gyro_y_cal variable
billwu1149 0:810f12542b58 83 gyro_z_cal += imu.gyroscope_data[2]; //Add the gyro z offset to the gyro_z_cal variable
billwu1149 0:810f12542b58 84 }
billwu1149 0:810f12542b58 85 // divide by 1000 to get avarage offset
billwu1149 0:810f12542b58 86 gyro_x_cal /= 1000;
billwu1149 0:810f12542b58 87 gyro_y_cal /= 1000;
billwu1149 0:810f12542b58 88 gyro_z_cal /= 1000;
billwu1149 0:810f12542b58 89
billwu1149 0:810f12542b58 90 clk.start();
billwu1149 0:810f12542b58 91 Tin = clk.read();
billwu1149 0:810f12542b58 92
billwu1149 0:810f12542b58 93 while(1) {
billwu1149 0:810f12542b58 94 imu.read_all(); //每次要讀取都要有這一項來宣告
billwu1149 0:810f12542b58 95 //lib中都已經事先除"轉換倍率"了
billwu1149 0:810f12542b58 96 meas1 = imu.gyroscope_data[0] - gyro_x_cal; // Converts and read the analog input value (value from 0.0 to 1.0
billwu1149 0:810f12542b58 97 meas2 = imu.gyroscope_data[1]- gyro_y_cal;
billwu1149 0:810f12542b58 98 meas3 = imu.gyroscope_data[2] - gyro_z_cal;
billwu1149 0:810f12542b58 99
billwu1149 0:810f12542b58 100 Xdeg = meas1*0.01*PI/180;//輸出為角度轉徑度
billwu1149 0:810f12542b58 101 Ydeg += meas2*0.01;//*PI/180;//0.002 = dt
billwu1149 0:810f12542b58 102 Zdeg = meas3*0.01*PI/180;
billwu1149 0:810f12542b58 103 Ydeg2=Ydeg;
billwu1149 0:810f12542b58 104
billwu1149 0:810f12542b58 105
billwu1149 0:810f12542b58 106 Axr = imu.accelerometer_data[0]; // Converts and read the analog input value (value from 0.0 to 1.0
billwu1149 0:810f12542b58 107 Ayr = imu.accelerometer_data[1]; //原始輸出都是以重力加速度g來看
billwu1149 0:810f12542b58 108 Azr = imu.accelerometer_data[2]; //
billwu1149 0:810f12542b58 109
billwu1149 0:810f12542b58 110 theta = -atan2(Axr,Azr)*180/PI;//
billwu1149 0:810f12542b58 111
billwu1149 0:810f12542b58 112 //==========kalman filter================
billwu1149 0:810f12542b58 113 Accel = theta;
billwu1149 0:810f12542b58 114 Gyro= meas2 ;
billwu1149 0:810f12542b58 115
billwu1149 0:810f12542b58 116 Angle+=(Gyro - Q_bias) * dt;
billwu1149 0:810f12542b58 117
billwu1149 0:810f12542b58 118 Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
billwu1149 0:810f12542b58 119
billwu1149 0:810f12542b58 120 Pdot[1]=- PP[1][1];
billwu1149 0:810f12542b58 121 Pdot[2]=- PP[1][1];
billwu1149 0:810f12542b58 122 Pdot[3]=Q_gyro;
billwu1149 0:810f12542b58 123
billwu1149 0:810f12542b58 124 PP[0][0] += Pdot[0] * dt;
billwu1149 0:810f12542b58 125 PP[0][1] += Pdot[1] * dt;
billwu1149 0:810f12542b58 126 PP[1][0] += Pdot[2] * dt;
billwu1149 0:810f12542b58 127 PP[1][1] += Pdot[3] * dt;
billwu1149 0:810f12542b58 128
billwu1149 0:810f12542b58 129 Angle_err = Accel - Angle;
billwu1149 0:810f12542b58 130
billwu1149 0:810f12542b58 131 PCt_0 = C_0 * PP[0][0];
billwu1149 0:810f12542b58 132 PCt_1 = C_0 * PP[1][0];
billwu1149 0:810f12542b58 133
billwu1149 0:810f12542b58 134 E = R_angle + C_0 * PCt_0;
billwu1149 0:810f12542b58 135
billwu1149 0:810f12542b58 136 if(E!=0)
billwu1149 0:810f12542b58 137 {
billwu1149 0:810f12542b58 138 K_0 = PCt_0 / E;
billwu1149 0:810f12542b58 139 K_1 = PCt_1 / E;
billwu1149 0:810f12542b58 140 }
billwu1149 0:810f12542b58 141
billwu1149 0:810f12542b58 142 t_0 = PCt_0;
billwu1149 0:810f12542b58 143 t_1 = C_0 * PP[0][1];
billwu1149 0:810f12542b58 144
billwu1149 0:810f12542b58 145 PP[0][0] -= K_0 * t_0;
billwu1149 0:810f12542b58 146 PP[0][1] -= K_0 * t_1;
billwu1149 0:810f12542b58 147 PP[1][0] -= K_1 * t_0;
billwu1149 0:810f12542b58 148 PP[1][1] -= K_1 * t_1;
billwu1149 0:810f12542b58 149
billwu1149 0:810f12542b58 150 Angle += K_0 * Angle_err;
billwu1149 0:810f12542b58 151 Q_bias += K_1 * Angle_err;
billwu1149 0:810f12542b58 152 Gyro_y = Gyro - Q_bias;
billwu1149 0:810f12542b58 153 //===========Alpha dot=============
billwu1149 0:810f12542b58 154 //alpha_dot = meas2*PI/180;//因為此處是設定以y軸當作轉軸來看(轉成徑度)
billwu1149 1:50963ee12158 155 kp=0.5;
billwu1149 1:50963ee12158 156 kd=0.2;
billwu1149 0:810f12542b58 157 angle1=Angle*PI/180;
billwu1149 0:810f12542b58 158 angle_dot=meas2*PI/180;
billwu1149 0:810f12542b58 159 u=kp*abs(angle1)+kd*abs(angle_dot);
billwu1149 1:50963ee12158 160 //pwm1=0.5+u;
billwu1149 1:50963ee12158 161 //pwm2=0.5-u;
billwu1149 0:810f12542b58 162 //u2=u*0.07*sin(angle1);
billwu1149 0:810f12542b58 163
billwu1149 1:50963ee12158 164 if (Angle>4)
billwu1149 0:810f12542b58 165 {
billwu1149 1:50963ee12158 166 pwm1=0.5+u;
billwu1149 1:50963ee12158 167 pwm2=0.5-u;
billwu1149 0:810f12542b58 168 }
billwu1149 0:810f12542b58 169
billwu1149 1:50963ee12158 170 else if (Angle<-4)
billwu1149 0:810f12542b58 171 {
billwu1149 1:50963ee12158 172 pwm1=0.5-u;
billwu1149 1:50963ee12158 173 pwm2=0.5+u;
billwu1149 0:810f12542b58 174 }
billwu1149 1:50963ee12158 175 else if (0<Angle<4 || 0>Angle>-4)
billwu1149 1:50963ee12158 176 {
billwu1149 1:50963ee12158 177 pwm1=0.5;
billwu1149 1:50963ee12158 178 pwm2=0.5;
billwu1149 1:50963ee12158 179 }
billwu1149 1:50963ee12158 180
billwu1149 0:810f12542b58 181
billwu1149 1:50963ee12158 182 servo1.write( pwm1);
billwu1149 1:50963ee12158 183 servo2.write( pwm2);
billwu1149 0:810f12542b58 184
billwu1149 0:810f12542b58 185 while((clk.read() - Tin)<0.01){} //設定取樣時間
billwu1149 0:810f12542b58 186 Tin = clk.read(); //Reset the loop timer
billwu1149 0:810f12542b58 187
billwu1149 0:810f12542b58 188
billwu1149 0:810f12542b58 189
billwu1149 1:50963ee12158 190 printf("angle_dot=%.3f ,Angle=%.3f\n",meas2,Angle);
billwu1149 1:50963ee12158 191 printf("u=%.3f\n",u);
billwu1149 0:810f12542b58 192
billwu1149 0:810f12542b58 193 }
billwu1149 0:810f12542b58 194 }