高级会员
- 积分
- 820
- 威望
- 400
- 贡献
- 244
- 兑换币
- 244
- 注册时间
- 2014-7-25
- 在线时间
- 88 小时
- 毕业学校
- 陕西理工学院
|
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include <math.h>
#define ushort unsigned short
#define uchar unsigned char
#define Motor_Dead 6
#define Motor_MAX 200
#define Motor_MIN -200
#define Gyrozhili 1329
#define Qjzhili 975
#define Zjmax 1605
#define Zjmin 675
#define Bu_Chang 3//2//时间补偿函数
#define Ji_Fen 20 //25//积分时间
#define gyro_pl 0.36////陀螺仪的比例系数
#define qingjiao_pl 0.193//0.27//加速度计的比例系数
float gyro_pi_out, // 陀螺仪的比例输出
qingjiao_pi_out, //加速度计的比例输出
gyro_single,
gyro_single_out,
Gyro_Data,
qingjiao,A,tuoluoyi; //角度输出
float angle_P=10.5, //直立PD控制参数P
angle_D=1, //直立PD控制参数D
speed_P=0.6,
speed_I=1;
int Left_Speed=0,
Right_Speed=0,
count_flag=0,
temp_left=0,
temp_right=0,
now_speed_L=0, //左测速
now_speed_R=0, //右测速
Cal_Left_Speed=0,
Cal_Right_Speed=0,
Speed_Old=0,
Speed_New=0,
Speed_Keep=0,
Speed_EPT=0, //速度预期
tly[20], tly1[6],
jsd[20],AD0_MAX,AD0_MIN,AD1_MIN,AD1_MAX
;
/****************************************/
//作用：设置单片机总线频率
//输入：无
//输出：无
/****************************************/
void SetBusCLK_80M(void)
{
CLKSEL_PLLSEL=0;//禁止锁相环时钟
PLLCTL_PLLON=1;//锁相环电路使能
SYNR=0xc0|0x09;//VCO_CLOCK=2*16M*(1+SYNR)/(1+REFDV)=128MHZ;
REFDV=0x80|0x01;//计算SYNR=10;REFDV=1;
POSTDIV=0x00;//设置fpll=fvoc;
//以下公式是进行计算
//fvco=2*16M*(1+SYNR)/(1+REFDV) OSTDIV=0x00的时候，fpll=fvco
//不等于零时fpll=fco/(2*POSTDIV);fbus=fpll/2;,
_asm(nop);
_asm(nop);
while(!(CRGFLG_LOCK==1));//时钟频率已锁定，锁相环频率锁定
CLKSEL_PLLSEL=1;//使能锁相环时钟
}
/****************************************/
//作用：配置PWM模块
//输入：无
//输出：无
/****************************************/
void PWM_Init(void)
{
PWME=0x00;//关闭所有PWM波通道
PWMPRCLK=0x00; //CLOCK A B总线不分频=80M
PWMSCLA=20; //对CLOCKA时钟40分频，SA的频率为2M
PWMSCLB=20; //对CLOCKB时钟40分频，SB的频率为2M
//*********第一路BTN7970的PWM波**********//左轮前进
PWME_PWME1=0;//
PWMCTL_CON01=1; //01通道级联使用
PWMPOL_PPOL1=1; //高电平翻转
PWMCLK_PCLK1=1; //选择SA作为时钟源SA=2M
PWMCAE_CAE1=0; //居中对齐
PWMPER01=200; //频率为10KHz
PWMDTY01=0;//
PWMCNT01=0x00; //01级联后计数器清零
PWME_PWME1=1 ; //使能01通道
//*************第二路BTN7960的PWM波************ //左轮后退就是反转
PWME_PWME3=0;//
PWMCTL_CON23=1; //23通道级联使用
PWMPOL_PPOL3=1; //高电平翻转
PWMCLK_PCLK3=1; //选择SA作为时钟源SA=2M
PWMCAE_CAE3=0; //居中对齐
PWMPER23=200; //频率为10KZ
PWMDTY23=0; //
PWMCNT23=0x00; //23级联后计数器清零
PWME_PWME3=1;//使能23通道
//**************第三路BTN7960的PWM波************//右轮前进
PWME_PWME5=0;//
PWMCTL_CON45=1; //45通道级联使用
PWMPOL_PPOL5=1; //高电平翻转
PWMCLK_PCLK5=1; //选择SA作为时钟源SA=2M
PWMCAE_CAE5=0; //居中对齐
PWMPER45=200; //频率为10khz
PWMDTY45=0; //
PWMCNT45=0x00; //45级联后计数器清零
PWME_PWME5=1;//使能45通道
//*************第四路BTN7960的PWM波***************//右轮后退就是反转
PWME_PWME7=0;//
PWMCTL_CON67=1; //45通道级联使用
PWMPOL_PPOL7=1; //高电平翻转
PWMCLK_PCLK7=1; //选择SA作为时钟源SA=2M
PWMCAE_CAE7=0; //居中对齐
PWMPER67=200; //频率为10KHZ
PWMDTY67=0; //
PWMCNT67=0x00; //45级联后计数器清零
PWME_PWME7=1;//使能67通道
}
/****************************************/
//作用：配置AD寄存器
//输入：无
//输出：无
/****************************************/
void ATD0_Init (void)
{
ATD0DIEN=0x00; //禁止数字输入功能
ATD0CTL0=0x06;
ATD0CTL1=0xC0; //分辨率为12位，采样前放电
ATD0CTL2=0x40; //快速清除标志位，禁止外部触发，禁止中断
ATD0CTL3=0xB0; //转换结果右对齐，每个序列转换6个通道。非FIF0
ATD0CTL4=0x1F; //采样时间4个周期，prs=19
ATD0CTL5=0x30; //对通道0-3连续采样，同时启动转换
}
/*void inittly() //对陀螺仪输出进行滤波使其平滑
{
int i,j,temp;
for(i = 0;i<6;i++) //0，1，2，3，4，5
{
while(!ATD0STAT2_CCF0);
tly1[i]=ATD0DR0;
}
for(i = 0;i<6;i++) //对数组中的6个结果进行比较大小
{
for(j = i+1;j<6;j++)
{
if(tly1[i]<tly1[j]) //按从大到小的顺序
{
temp = tly1[j];
tly1[j] = tly1[i];
tly1[i] = temp;
}
}
}
tuoluoyi = (tly1[1]+tly1[2]+tly1[3]+tly1[4])/4;//取较大的四个值求平均值
}*/
/****************************************/
//作用：配置定时器模块寄存器
//输入：无
//输出：无
/****************************************/
void PIT_Init(void)//我们这一块定义1ms的采样周期，对于外部的AD采样周期
{
PITCFLMT=0x00;//禁止定时器模块
PITCE_PCE0=1;//使能定时器0
PITMUX=0x00;//使用16位定时器，使用微定时基准0计数
PITMTLD0=80-1;//装入8位微定时初值，这个数值由我们定时时间决定
PITLD0=1000-1;//装入16位计数的初值，这个数值也由我们的定时时间决定
//定时时间的计算。我们假设定时时间为t=(PITMTLD0+1)*(PITLD0+1)*(1/fbus)
//总线时钟我们在锁相环中已经设定好好了， fbus=80Mhz
PITINTE=0x01;//使能定时器通道0中断
PITCFLMT=0x80;//使能定时器模块
}
void TimInit (void)
{
PACTL=0X00;
PACNT=0X0000;//设置脉冲累加器初值
PACTL_PAEN=1;
}
void IO_Init(void)
{
DDRM=0XFF;
PTM_PTM0=1;
PTM_PTM1=0;
}
/*************************************************************/
/* AD采集函数 */
/*************************************************************/
void ADcaiji(void)
{
int i,j;
for(i = 0;i<=19;i++)
{
while(!ATD0STAT2_CCF0);
tly[i]=ATD0DR0;
}
AD0_MAX=tly[0];//对AD0_MAX附初值
AD0_MIN=tly[0];//对AD0_MIN附初值
for(i = 0;i<=19;i++) {
if(tly[i]>AD0_MAX)
{
AD0_MAX=tly[i] ; //将数组AD0中的值与初值比较找出最大值
}
else if (tly[i]<AD0_MIN)
{
AD0_MIN=tly[i] ; //将数组AD0中的值与初值比较找出最小值
}
}
Gyro_Data =(tly[0]+tly[1]+tly[2]+tly[3]+tly[4]+tly[5]+tly[6]+tly[7]+tly[8]+tly[9]+tly[10]+tly[11]+tly[12]+tly[13]+tly[14]+tly[15]+tly[16]+tly[17]+tly[18]+tly[19]-AD0_MAX-AD0_MIN)/18;
for(j =0;j<=19;j++)
{
while(!ATD0STAT2_CCF1);
jsd[j]=ATD0DR1;
}
AD1_MAX=jsd[0];
AD1_MIN=jsd[0];
for(j =0;j<=19;j++) {
if(jsd[j]>AD1_MAX)
{
AD1_MAX=jsd[j] ; //将数组AD0中的值与初值比较找出最大值
}
else if (jsd[j]<AD1_MIN)
{
AD1_MIN=jsd[j] ; //将数组AD0中的值与初值比较找出最小值
}
}
qingjiao = (jsd[0]+jsd[1]+jsd[2]+jsd[3]+jsd[4]+jsd[5]+jsd[6]+jsd[7]+jsd[8]+jsd[9]+jsd[10]+jsd[11]+jsd[12]+jsd[13]+jsd[14]+jsd[15]+jsd[16]+jsd[17]+jsd[18]+jsd[19]-AD1_MAX-AD1_MIN)/18;//采集20次取平均值
}
void kalman_update(void)
{
float Q =1,R =60;
static float RealData = 0,RealData_P = 0;
float NowData = 0,NowData_P = 0;
float Kg = 0,accelerometer_angle=0;
accelerometer_angle=qingjiao; // 请注意 这个赋值，为了输出，任何时刻仅有一个
NowData = RealData ;
NowData_P = sqrt(Q*Q+RealData_P*RealData_P);
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R));
RealData = NowData + Kg*(accelerometer_angle - NowData);
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);
qingjiao = RealData;
}
/*************************************************************/
/* 将陀螺仪得到角速度数值和加速度计的得到的数值进行拟合 */
/*************************************************************/
void gyroscope_angle(void)
{ float qingjiao_bu_out;
qingjiao_pi_out=(qingjiao-Qjzhili)*qingjiao_pl;//加速度计比例输出
// gyro_pi_out=(-Gyro_Data+tuoluoyi)*gyro_pl;//陀螺仪比例输出
gyro_pi_out=(-Gyro_Data+Gyrozhili)*gyro_pl;//陀螺仪比例输出
gyro_single=gyro_single_out;
qingjiao_bu_out=(qingjiao_pi_out-gyro_single)/Bu_Chang;//时间补偿
gyro_single_out+=(qingjiao_bu_out+gyro_pi_out)/Ji_Fen;//角度输出
gyro_single_out=gyro_single_out;
}
void kalman_update1(void)
{
float Q =1,R =1;
static float RealData = 0,RealData_P = 0;
float NowData = 0,NowData_P = 0;
float Kg = 0,accelerometer_angle=0;
accelerometer_angle=gyro_single_out; // 请注意 这个赋值，为了输出，任何时刻仅有一个
NowData = RealData ;
NowData_P = sqrt(Q*Q+RealData_P*RealData_P);
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R));
RealData = NowData + Kg*(accelerometer_angle - NowData);
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);
gyro_single_out= RealData;
}
/*************************************************************/
/* 直立PD */
/*************************************************************/
void CarAngleAdjust(void)
{
int nLeft=0,
nRight=0,
nSpeed=0,
nP=0,
nD=0;
nP=(-gyro_single_out)*angle_P;
nD=(-gyro_pi_out)*angle_D;
nSpeed=nP+nD;
Left_Speed=nSpeed+Cal_Left_Speed;
Right_Speed=nSpeed+Cal_Right_Speed;
if((gyro_single_out>45)||(gyro_single_out<-45))
{
Left_Speed=0;
Right_Speed=0;
}
}
/*************************************************************/
/* 测速函数 */
/*************************************************************/
void CarSpeedGet(void)
{
PTM_PTM0=~PTM_PTM0;
PTM_PTM1=~PTM_PTM1;
if(PTM_PTM0==1)
{
if(Left_Speed>0)
temp_left+=PACNT;
else
temp_left-=PACNT;
}
else
{
if(Right_Speed>0)
temp_right+=PACNT;
else
temp_right-=PACNT;
}
PACNT=0;
count_flag++;
}
/*************************************************************/
/* 速度PI控制 */
/*************************************************************/
void CarMotorSpeedAdjust(void)
{
// float SpeedOut;
// float EK0,EK1;
int nLeftSpeed=0,
nRightSpeed=0,
nP=0,
nI=0,
nSpeed=0,
nSpeedChang=0;
nLeftSpeed=(int)now_speed_L;
nRightSpeed=(int)now_speed_R;
nSpeed=(nLeftSpeed+nRightSpeed)/2;
nSpeedChang=Speed_EPT-nSpeed;
nP=nSpeedChang*speed_P;
nI=nSpeedChang*speed_I;
Speed_Old=Speed_New;
Speed_Keep-=nI;
Speed_New=(Speed_Keep>>3)-nP;
if(Speed_Keep>Motor_MAX)
Speed_Keep=Motor_MAX;
if(Speed_Keep<Motor_MIN)
Speed_Keep=Motor_MIN;
}
void CarMotorSpeedOutCal(void)
{
int nSpeedOut;
nSpeedOut=Speed_New-Speed_Old;
nSpeedOut=nSpeedOut*(count_flag/20)+Speed_Old;
Cal_Left_Speed=Cal_Right_Speed=nSpeedOut;
}
/*************************************************************/
/* 电机控制 */
/*************************************************************/
void SetMotor(void)
{
int T_Left_Speed,T_Right_Speed;
T_Left_Speed=Left_Speed;
T_Right_Speed=Right_Speed;
if(T_Left_Speed>=0)
{
T_Left_Speed+=Motor_Dead;
if( T_Left_Speed>Motor_MAX)
T_Left_Speed=Motor_MAX;
PWMDTY45=T_Left_Speed;
PWMDTY67=0;
}
else
{
T_Left_Speed=-T_Left_Speed;
T_Left_Speed+=Motor_Dead;
if( T_Left_Speed>Motor_MAX)
T_Left_Speed=Motor_MAX;
PWMDTY67=T_Left_Speed;
PWMDTY45=0;
}
if(T_Right_Speed>=0)
{
T_Right_Speed+=Motor_Dead;
if( T_Right_Speed>Motor_MAX)
T_Right_Speed=Motor_MAX;
PWMDTY01=T_Right_Speed;
PWMDTY23=0;
}
else
{
T_Right_Speed=- T_Right_Speed;
T_Right_Speed+=Motor_Dead;
if( T_Right_Speed>Motor_MAX)
T_Right_Speed=Motor_MAX;
PWMDTY23=T_Right_Speed;
PWMDTY01=0;
}
}
/*************************************************************/
/* 初始化 */
/*************************************************************/
void DeviceInit()
{
SetBusCLK_80M();
COPCTL&=0XF8;
ATD0_Init();
// inittly();
PWM_Init();
PIT_Init();
TimInit();
IO_Init();
}
void delay (int x)
{
int m,n;
for (m=x;m>0;m--)
for (n=200;n>0;n--);
}
/*************************************************************/
/* 主函数 */
/*************************************************************/
void main(void)
{
DisableInterrupts; //关闭所有中断
DeviceInit();//所有装置初始化
delay (1000);
EnableInterrupts;
for(;;)
{
_FEED_COP(); /* feeds the dog */
} /* loop forever */
}
/*************************************************************/
/* 1M定时中断 */
/*************************************************************/
#pragma CODE_SEG __NEAR_SEG NON_BANKED //指示该程序在不分页区
void interrupt 66 PIT0(void)
{
ADcaiji();//获取传感器信息
kalman_update();
gyroscope_angle();//角度滤波
kalman_update1();
CarAngleAdjust();//角度调整
CarSpeedGet();
if(count_flag==20)
{
now_speed_L=temp_left;
now_speed_R=temp_right;
temp_left=0;
temp_right=0;
CarMotorSpeedAdjust();
count_flag=0;
}
CarMotorSpeedOutCal();
SetMotor();
//直立电机控制
PITTF_PTF0=1;//清中断标志位
}
跪求大神！！！！
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
|
发表于 2015-4-7 19:51:19
微信
收藏
转播
分享
顶
踩
提升卡
变色卡
抢沙发
楼主
