（最终版）(1) - 图文(8)

附录2：系统总原理图

36

附录3：单片机最小系统PCB图

附录4：两轮车主程序

#include #include #include #include

typedef unsigned char uchar; typedef unsigned short ushort; typedef unsigned int uint;

//******功能模块头文件*******

#include \.H\ #include \#include \#include \#include \

#include \

//延时头文件

//MPU6050头文件 //PWM头文件

//串口头文件 //卡尔曼滤波头文件

//AD转换头文件

37

//******角度参数************

float Angle_gx; //由角速度计算的倾斜角度 float Accel_y; uchar value;

//X轴加速度值暂存

//角度正负极性标记

float Angle_ay; //由加速度计算的倾斜角度 float Angle; //小车最终倾斜角度 float Gyro_x; //Y轴陀螺仪数据暂存

//******PWM参数*************

int PWM_R; //右轮PWM值计算 int PWM_L; //左轮PWM值计算 float PWM; //综合PWM计算 float PWMI;

//PWM积分值

int speed_mr; int speed_ml; char turn_need=0; char speed_need;

//******电机参数*************

float speed_r_l; //电机转速 float speed; //电机转速滤波 float position;

//******定时器，外部中断初始化************* void InitTimer0(void) {

TMOD = TMOD|0x01; TH0 = 0x0B1; TL0 = 0x0E0; EA = 1; ET0 = 1; TR0 = 1; }

void Init_exint(void) {

IT1=1;

IT0=1; EX1=1; EX0=1;

//位移

float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;

//右电机转速 //左电机转速

38

}

//******角度，角速度处理函数********** void Angle_Calcu(void) {

//-------角速度------------------------- }

//********************************************************* //电机转速和位移值计算

//*********************************************************

void Psn_Calcu(void) {

speed_r_l =(speed_mr + speed_ml)*0.5; speed *= 0.7;

//车轮速度滤波

//积分得到位移

speed += speed_r_l*0.3; position += speed; position += speed_need;

if(position<-6000) position = -6000; if(position> 6000) position = 6000;

Kalman_Filter(Angle_ay,Gyro_x); //卡尔曼滤波计算倾角

Gyro_x = GetData(GYRO_XOUT_H); //静止时角速度Y轴输出为-30左右 Gyro_x =(Gyro_x-8) /16.4; //去除零点偏移，计算角速度值,负号为方向处理 //Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.

//-------卡尔曼滤波融合-----------------------

//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s) Accel_y = GetData(ACCEL_YOUT_H);

//读取X轴加速度

Angle_ay = (Accel_y - 1100) /16384; //去除零点偏移,计算得到角度（弧度） Angle_ay = Angle_ay*1.2*180/3.14; //弧度转换为度, //范围为2g时，换算关系：16384 LSB/g

//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14 //因为x>=sinx,故乘以1.3适当放大 //------加速度--------------------------

39

}

//********************************************************* //电机PWM值计算

//*********************************************************

static float code Kp = 9.0; //PID参数 static float code Kd = 2.6; //PID参数 static float code Kpn = 0.01; //PID参数 static float code Ksp = 2.0; //PID参数

void PWM_Calcu(void) {

if(Angle<-40||Angle>40) //角度过大，关闭电机 {

CCAP0H = 0; return; }

CCAP1H = 0;

PWM = Kp*Angle*K_angle_AD + Kd*Gyro_x*K_angle_dot_AD; //PID：角速度和角度 PWM += Kpn*position*K_position_AD + Ksp*speed*K_position_dot_AD; //PID：速度和位置 PWM_R = PWM - turn_need; PWM_L = -PWM - turn_need; PWM_output(PWM_L,PWM_R); }

//******主函数************* void main() {

InitMPU6050(); //初始化MPU6050 InitTimer0(); Init_exint(); serial_init(); AD_init(); PWM_init(); while(1) {

Angle_Calcu();

Psn_Calcu(); //电机位移计算 PWM_Calcu();

delaynms(500);

40

}

}

Sendstring(\ \Sendstring(\ \Sendstring(\Put_Ser_Dis(Gyro_x); Sendstring(\Sendstring(\Put_Ser_Dis(PWM); Sendstring(\Senddata(0x0d); Senddata(0x0a);

Put_Ser_Dis(Angle_ay);

//******定时器中断************* void Timer0Interrupt(void) interrupt 1 {

TH0 = 0x0B1; TL0 = 0x0E0;

speed_mr = speed_ml = 0; }

//********左电机中断*********************** void INT_L(void) interrupt 0 {

if(RA == 1) { speed_ml++; } else }

//********右电机中断***********************

void INT_R(void) interrupt 2 {

if(RB == 1) { speed_mr++; } else }

//右电机前进

//右电机后退

{ speed_mr--; }

//左电机前进 //左电机后退

{ speed_ml--; }

K_angle_AD=AD_work(0)/6.0; K_angle_dot_AD=AD_work(1)/6.0; K_position_AD=AD_work(6)/6.0; K_position_dot_AD=AD_work(7)/6.0;

//add your code here!

41