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?
User | Revision | Line number | New 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 | } |