用stc12可以做平衡车吗？？ -

/***********************************************************************
// 两轮自平衡车最终版控制程序（6轴MPU6050+互补滤波+PWM电机）
// 单片机STC12C5A60S2
// 晶振：20M
// 日期：2014.11.26 - ？
***********************************************************************/
#include <REG52.H>
#include <math.h>
#include <stdio.h>
#include <INTRINS.H>
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//******功能模块头文件*******
#include "DELAY.H" //延时头文件
//--------------------------------------------
#include "STC_ISP.H" //程序烧录头文件
#include "SET_SERIAL.H"//串口头文件
#include "SET_PWM.H"//PWM头文件
#include "MOTOR.H"//电机控制头文件
#include "MPU6050.H"//MPU6050头文件
//-----------------这些文件---------------------------
//******角度参数************
float Gyro_y; //Y轴陀螺仪数据暂存
//--------------加速度和角度的转化------------------
float Angle_gy; //由角速度计算的倾斜角度
//陀螺仪直接反应角度
float Accel_x; //X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度、
float Angle; //小车最终倾斜角度
uchar value; //角度正负极性标记
//******PWM参数*************
int speed_mr; //右电机转速//这个的测量---------转盘
int speed_ml; //左电机转速
int PWM_R; //右轮PWM值计算
int PWM_L; //左轮PWM值计算
float PWM; //综合PWM计算
float PWMI; //PWM积分值
//******电机参数*************
float speed_r_l;//电机转速
float speed; //电机转速滤波
float position; //位移
//******蓝牙遥控参数*************
uchar remote_char;
char turn_need;
char speed_need;
//*********************************************************
//定时器100Hz数据更新中断 T1定时器
//*********************************************************
void Init_Timer1(void)//10毫秒@20MHz,100Hz刷新频率
{
AUXR &= 0xBF;//定时器时钟12T模式
TMOD &= 0x0F;//设置定时器模式
TMOD |= 0x10;//设置定时器模式
TL1 = 0xE5; //设置定时初值
TH1 = 0xBE; //设置定时初值
TF1 = 0; //清除TF1标志
TR1 = 1; //定时器1开始计时
}
//*********************************************************
//中断控制初始化
//*********************************************************
void Init_Interr(void)
{
EA = 1; //开总中断
EX0 = 1; //开外部中断INT0
EX1 = 1; //开外部中断INT1
IT0 = 1; //下降沿触发
IT1 = 1; //下降沿触发
ET1 = 1; //开定时器1中断
}
//******卡尔曼参数************
float code Q_angle=0.001;
float code Q_gyro=0.003;
float code R_angle=0.5;
float code dt=0.01; //dt为kalman滤波器采样时间;
char code C_0 = 1;
float xdata Q_bias, Angle_err;
float xdata PCt_0, PCt_1, E;
float xdata K_0, K_1, t_0, t_1;
float xdata Pdot[4] ={0,0,0,0};
float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };
//*********************************************************
// 卡尔曼滤波
//*********************************************************
//Kalman滤波，20MHz的处理时间约0.77ms；
void Kalman_Filter(float Accel,float Gyro) // 滤波，输出标准的方波驱动电机
{
Angle+=(Gyro - Q_bias) * dt; //先验估计陀螺角度
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;//zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_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; //输出值(后验估计)的微分=角速度
}
//*********************************************************
// 倾角计算（卡尔曼融合）
//*********************************************************
void Angle_Calcu(void)
{
//------加速度--------------------------
//范围为2g时，换算关系：16384 LSB/g
//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因为x>=sinx,故乘以1.3适当放大
Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度存在ACCEL_XOUT_H 寄存器
Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,
//-------角速度-------------------------
//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为【-30】左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值, 【负号为方向处理】
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.
//-------卡尔曼滤波融合-----------------------
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
/*//-------互补滤波-----------------------
//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度
//0.5为放大倍数，可调节补偿度；0.01为系统周期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}
//*********************************************************
//电机转速和位移值计算
//*********************************************************
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;
}
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参数
//*********************************************************
//电机PWM值计算
//*********************************************************
void PWM_Calcu(void)
{
if(Angle<-40||Angle>40) //角度过大，关闭电机
{
CCAP0H = 0;
CCAP1H = 0;
return;
}
PWM = Kp*Angle + Kd*Gyro_y; //PID：角速度和角度
PWM += Kpn*position + Ksp*speed; //PID：速度和位置
PWM_R = PWM + turn_need;
PWM_L = PWM - turn_need; //蓝牙的倾斜，
PWM_Motor(PWM_L,PWM_R);
}
//*********************************************************
//手机蓝牙遥控
//*********************************************************
void Bluetooth_Remote(void)
{
remote_char = receive_char(); //接收蓝牙串口数据
if(remote_char ==0x02) speed_need = -80; //前进
else if(remote_char ==0x01) speed_need = 80; //后退
else speed_need = 0; //不动
if(remote_char ==0x03) turn_need = 15; //左转
else if(remote_char ==0x04) turn_need = -15; //右转
else turn_need = 0; //不转
}
/*=================================================================================*/
//*********************************************************
//main
//*********************************************************
void main()
{
delaynms(500); //上电延时
Init_PWM(); //PWM初始化
Init_Timer0(); //初始化定时器0，作为PWM时钟源
Init_Timer1(); //初始化定时器1
Init_Interr(); //中断初始化
Init_Motor(); //电机控制初始化
Init_BRT(); //串口初始化（独立波特率）
InitMPU6050(); //初始化MPU6050
delaynms(500);
while(1)
{
Bluetooth_Remote();
}
}
/*=================================================================================*/
//********timer1中断***********************
void Timer1_Update(void) interrupt 3
{
TL1 = 0xE5; //设置定时初值10MS
TH1 = 0xBE;
//STC_ISP(); //程序下载
Angle_Calcu(); //倾角计算
Psn_Calcu(); //电机位移计算
PWM_Calcu(); //计算PWM值
speed_mr = speed_ml = 0;
}
//********右电机中断***********************
void INT_L(void) interrupt 0
{
if(SPDL == 1) { speed_ml++; } //左电机前进
else { speed_ml--; } //左电机后退
LED = ~LED;
}
//********左电机中断***********************
void INT_R(void) interrupt 2
{
if(SPDR == 1) { speed_mr++; } //右电机前进
else { speed_mr--; } //右电机后退
LED = ~LED;
}

复制代码

/*--------------------------------------------------------------------------
DELAY.H
延时函数头文件
--------------------------------------------------------------------------*/
#ifndef __DELAY_H__
#define __DELAY_H__
#define uchar unsigned char
#define uint unsigned int
//------函数（声明）--------------------------------
void Delay400Ms(void); //延时400mS函数（子程序）
void delay1ms(void); //延时1mS函数（子程序）
void delaynms(uchar n); //延时n mS函数（子程序）
//***********************************************************************************/
//延时400ms
void Delay400Ms(void)
{
uchar TempCycA = 30;//1周期MPU用30, 12周期MPU用5
uint TempCycB;
while(TempCycA--)
{
TempCycB=7269;
while(TempCycB--);
};
}
//***********************************************************************************/
//延时1ms
void delay1ms()
{
uchar i,j;
for(i=0;i<10;i++)
for(j=0;j<33;j++)
;
}
//***********************************************************************************/
//延时n*ms
void delaynms(uchar n)
{
uchar i;
for(i=0;i<n;i++) delay1ms();
}
#endif

复制代码

/*****************************************************
普通IO模拟I2C通信
STC12C5A60S2 IT单片机 AXTL;11.0592MHz
*****************************************************/
#ifndef __I2C_H__
#define __I2C_H__
#include <REG52.H>
#include <math.h> //Keil library
#include <stdio.h> //Keil library
#include <INTRINS.H>
//********端口定义**********************************
sbit SCL=P2^1;//IIC时钟引脚定义
sbit SDA=P2^0;//IIC数据引脚定义
#defineSlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
//********函数初始定义******************************
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
void display_ACCEL_x();
void display_ACCEL_y();
void display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address);//读取I2C数据
void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据
//**************************************
//延时5微秒(STC12C5A60S2@12M)
//不同的工作环境,需要调整此函数
//此延时函数是使用1T的指令周期进行计算,与传统的12T的MCU不同
//**************************************/
void Delay5us()
{
uchar n = 4;
while (n--)
{
_nop_();
_nop_();
}
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 0; //产生下降沿
Delay5us(); //延时
SCL = 0; //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 1; //产生上升沿
Delay5us(); //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //写应答信号
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay5us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay5us(); //延时
return CY;
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
I2C_RecvACK();
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1;
SCL = 1; //拉高时钟线
Delay5us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
return dat;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址
I2C_SendByte(REG_data); //内部寄存器数据
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址，从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
I2C_SendACK(1); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
#endif

复制代码

/*********************************************************
STC_12C5A60S2免断电烧录程序
*********************************************************/
#ifndef _STC_ISP_H_
#define _STC_ISP_H_
#include<REG52.h>
sfr IAP_CONTR = 0xC7; //IAP Control Register
sbit IN_OFF=P3^0;//串口接收端
//**************************************************
void STC_ISP()
{
IN_OFF=1; //――2
if(!IN_OFF){
IAP_CONTR=0x60;
} //判断串口是否有数据过来
}
#endif

复制代码

/**********************************************************************
串口收发程序（头文件）
波特率：9600Hz
可选定时器1或独立波特率作为串口时钟
**********************************************************************/
#ifndef __SET_SERIAL_H__
#define __SET_SERIAL_H__
sfr AUXR = 0x8E;
sfr BRT = 0x9C;
//***********************************************************************************/
//定时器1初始化
/*void Init_Timer1()
{
// set timer1 mode
TMOD |= 0x20; //设置timer1为8位自动重载模式
SCON = 0x50; // 设定串行口工作方式
PCON &= 0xef; // 波特率不倍增
TH1=TL1=0xFD; //9600 Hz
TR1 = 1; //开启Timer1
}*/
//***********************************************************************************/
//独立波特率初始化
void Init_BRT()////9600bps@20MHz
{
PCON &= 0x7F;//波特率不倍速
SCON = 0x50; //8位数据,可变波特率
AUXR |= 0x04;//独立波特率发生器时钟为Fosc,即1T
BRT = 0xBF; //设定独立波特率发生器重装值
AUXR |= 0x01;//串口1选择独立波特率发生器为波特率发生器
AUXR |= 0x10;//启动独立波特率发生器
}
//***********************************************************************************/
//串口接收程序
receive_char() //receive data
{
unsigned char rxd;
if(RI)
{
RI = 0;
rxd = SBUF;
return(rxd);
}
}
//***********************************************************************************/
//串口发送子程序
void send_char(unsigned char txd) //sent out data
{
SBUF = txd;
while(!TI);
TI = 0;
}
#endif

复制代码

/*********************************************************************************************
两路PWM输出控制设置
/*********************************************************************************************/
#ifndef __SET_PWM_H__
#define __SET_PWM_H__
//Declare SFR associated with the PCA
sfr CCON = 0xD8; //PCA control register
sbit CCF0 = CCON^0; //PCA module-0 interrupt flag
sbit CCF1 = CCON^1; //PCA module-1 interrupt flag
sbit CR = CCON^6; //PCA timer run control bit
sbit CF = CCON^7; //PCA timer overflow flag
sfr CMOD = 0xD9; //PCA mode register
sfr CL = 0xE9; //PCA base timer LOW
sfr CH = 0xF9; //PCA base timer HIGH
sfr CCAPM0 = 0xDA; //PCA module-0 mode register
sfr CCAP0L = 0xEA; //PCA module-0 capture register LOW
sfr CCAP0H = 0xFA; //PCA module-0 capture register HIGH
sfr CCAPM1 = 0xDB; //PCA module-1 mode register
sfr CCAP1L = 0xEB; //PCA module-1 capture register LOW
sfr CCAP1H = 0xFB; //PCA module-1 capture register HIGH
sfr PCAPWM0 = 0xf2;
sfr PCAPWM1 = 0xf3;
//***********************************************************************************/
//PWM模式设置
void Init_PWM()
{
// set PWM mode
CCON = 0; //Initial PCA control register(PCA timer stop,Clear CF flag,Clear all module interrupt flag)
CL = 0; //Reset PCA base timer
CH = 0;
CMOD = 0x04; //Set PCA timer clock source as timer0 overflow,Disable PCA timer overflow interrupt
CCAP0H = CCAP0L = 255; //PWM0 port output X% duty cycle square wave
CCAPM0 = 0x42; //PCA module-0 work in 8-bit PWM mode and no PCA interrupt
CCAP1H = CCAP1L = 255; //PWM1 port output X% duty cycle square wave
CCAPM1 = 0x42; //PCA module-1 work in 8-bit PWM mode and no PCA interrupt
CR = 1; //PCA timer start run
}
//***********************************************************************************/
//设置Timer0为8位自动重载模式，作为PWM时钟源
void Init_Timer0()
{
// set timer0 mode
AUXR = 0x00; //timer0 work in 12T mode
TMOD|= 0x02; //set timer0 counter mode2 (8-bit auto-reload)
TH0=TL0=130; //PWM 50Hz
TR0 = 1; //timer0 start running(as PWM clk)
}
#endif

复制代码

/****************************************
平衡车两路直流减速电机PWM控制
****************************************/
#ifndef__MOTOR_H__
#define __MOTOR_H__
#include <REG52.H>
#include "SET_PWM.H"//PWM头文件
//电机PWM控制端口定义
sbit LED = P0^0; //LED
sbit EN12 = P1^3; //L298电机（左）驱动使能
sbit EN34 = P1^4; //L298电机（右）驱动使能
sbit M1= P1^1; //电机驱动(M1=1，M2=0，右电机后退)
sbit M2= P1^2; //电机驱动(M1=0，M2=1，右电机前进)
sbit M3= P1^5; //电机驱动(M3=1，M4=0，左电机前进)
sbit M4= P1^0; //电机驱动(M3=0，M4=1，左电机后退)
sbit SPDR = P3^4; //右电机B相测速，用来判断电机转向
sbit SPDL = P3^5; //左电机B相测速，用来判断电机转向
//变量定义
bit flg_direc; //电机运动方向标志
/*********函数区************************/
//**************************************
//电机初始化
//**************************************
void Init_Motor()
{
CCAP0H = 255; //关闭PWM0输出
CCAP1H = 255; //关闭PWM1输出
EN12 = 0; //关闭左电机
EN34 = 0; //关闭右电机
}
//**************************************
//电机控制
//**************************************
void PWM_Motor(int pwm_l,int pwm_r)
{
if(pwm_l<0)
{
M1 = 1; //右电机后退
M2 = 0;
pwm_l = -pwm_l;
}
else
{
M1 = 0; //右电机前进
M2 = 1;
}
if(pwm_r<0)
{
M3 = 0; //左电机后退
M4 = 1;
pwm_r = -pwm_r;
}
else
{
M3 = 1; //左电机前进
M4 = 0;
}
if(pwm_l>255) pwm_l = 255 ; //防止PWM值超过255
if(pwm_r>255) pwm_r = 255 ; //防止PWM值超过255
CCAP0H = 0 + pwm_l; //设定PWM0占空比(CCAP0H=255，速度最大)
CCAP1H = 0 + pwm_r; //设定PWM1占空比(CCAP0H=255，速度最大)
//这里50可调，作用是跳过电机PWM死区，在PWM值为0-40时，电机不转动，即死区（**错误的注释，解释在下句*）
//*此处曾经完全被误导，曾经在0的地方加的是50，目的是为了消除电机PWM死区，但是导致小车严重震荡，现在该为零才对
}
#endif

复制代码

/*********************************************
MPU-6050数据处理
*********************************************/
#ifndef _MPU6050_H_
#define _MPU6050_H_
#include <REG52.H>
#include <INTRINS.H>
#include "I2C.H"
//****************************************
// 定义MPU6050内部地址
//****************************************
#defineSMPLRT_DIV0x19//陀螺仪采样率，典型值：0x07(125Hz)
#defineCONFIG0x1A//低通滤波频率，典型值：0x06(5Hz)
#defineGYRO_CONFIG0x1B//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#defineACCEL_CONFIG0x1C//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#defineACCEL_XOUT_H0x3B
#defineACCEL_XOUT_L0x3C
#defineACCEL_YOUT_H0x3D
#defineACCEL_YOUT_L0x3E
#defineACCEL_ZOUT_H0x3F
#defineACCEL_ZOUT_L0x40
#defineTEMP_OUT_H0x41
#defineTEMP_OUT_L0x42
#defineGYRO_XOUT_H0x43
#defineGYRO_XOUT_L0x44
#defineGYRO_YOUT_H0x45
#defineGYRO_YOUT_L0x46
#defineGYRO_ZOUT_H0x47
#defineGYRO_ZOUT_L0x48
#definePWR_MGMT_10x6B//电源管理，典型值：0x00(正常启用)
#defineWHO_AM_I0x75//IIC地址寄存器(默认数值0x68，只读)
//#defineSlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
//MPU6050操作函数声明
void InitMPU6050(); //初始化MPU6050
int GetData(uchar REG_Address); //16位数据合成
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00);//解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//**************************************
//合成数据
//**************************************
int GetData(uchar REG_Address)
{
uint H;
uchar L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L; //合成数据
}
#endif

复制代码