程序.docx
《程序.docx》由会员分享，可在线阅读，更多相关《程序.docx（16页珍藏版）》请在冰豆网上搜索。
程序
#include/*commondefinesandmacros*/
#include
#include"SCI.h"
#include"SET_PLL.h"
#include"Init.h"
#include"ATD.h"
#include"Init.h"
#include"PWM.h"
#include"CollectData.h"
#include"Serial.h"
#include"PID.h"
/***********锁相环初始化**********/
voidPll_Init（）//完成Pll初始化
{
CLKSEL=0X00;
REFDV=0X00|0X07;
SYNR=0x40|0x13;
PLLCTL_PLLON=1;
_asm（nop）;
while（!
（CRGFLG_LOCK==1））
POSTDIV=0X00;
CLKSEL_PLLSEL=1;
}
/***********PIT定时中断初始化**********/
voidPit_Init（void）//完成PIT初始化
{
PITCFLMT_PITE=0;//初始化前先关闭PIT模块
PITCE_PCE0=1;//定时器0通道使能
PITMUX_PMUX0=0;//使用微计数器0
PITMTLD0=40-1;//8位定时器初值
PITLD0=1000-1;//16位定时器初值设定1MS
PITINTE_PINTE0=1;//定时器溢出中断使能
PITCFLMT_PITE=1;//定时器通道使能，开启PIT模块
}
/***********串口初始化**********/
voidSci_Init（void）//SCI初始化
{
SCI0BD=260;//9600bpsBaudRate=BusClock/（16*SCIBD）
SCI0CR1=0x00;//正常8位模式，无奇偶校验
SCI0CR2=0x0C;//发送允许接受中断允许
}
/***********ATD_Init（）**********/
voidATD_Init（）
{
ATD0CTL0=0x00;
ATD0CTL1=0x40;//7:
1-外部触发,65:
00-8位精度,4:
放电,3210:
ch
ATD0CTL2=0x40;//禁止外部触发,中断禁止模块标志快速清除ATD0CTL3=0x80|0x48;
ATD0CTL4=0x0A;//PRS=10ATDClock=[BusClock*0.5]/[PRS+1]=88M*0.5/[10+1]=4M
ATD0DIEN=0x00;//禁止数字输入
}
/***********PWM初始化**********/
voidPWM_Init（）
{
PWME=0x00;//PWM禁止
PWMPRCLK=0x00;//
PWMSCLA=4;//ClockSA=ClockA/（2*PWMSCLA）SA频率为40/（2*4）=5MHz
PWMSCLB=4;//ClockSB=ClockA/（2*PWMSCLB）SB频率为40/（2*4）=5MHz
PWMCLK=0xAA;//通道1选择SA通道3选择SB通道5选择SA通道7选择SB
PWMCTL=0xF0;//通道级联
PWMCAE=0x00;//左对齐
PWMPOL=0xAA;//高电平-->低电平
PWMDTY01=0;//左正转
PWMPER01=1000;//5KHZ
PWMDTY23=0;//左反转
PWMPER23=1000;//5KHZ
PWMDTY67=0;//右正转
PWMPER67=1000;//5KHZ
PWMDTY45=0;//右反转
PWMPER45=1000;//5KHZ（50--200HZ）
PWME=0xAA;
}
/***********输入捕捉初始化程序**********/
voidTim_Init（void）
{
TCNT=0x00;
TSCR1=0x80;//时钟允许,定时器使能
TSCR2=0x04;//divby16总线频率/16分频
PACTL=0x40;//PT7PIN,PACN3216BIT,FALLingedge禁止脉冲累加，事件计数，
TCTL3=0x00;//通道0，1，上升沿捕捉
TCTL4=0x05;//通道0，1，上升沿捕捉
TIE=0x03;//通道中断使能
TIOS=0x00;//每一位对应通道的:
0输入捕捉,1输出比较
}
/***********1ms定时中断服务程序**********/
#pragmaCODE_SEG__NEAR_SEGNON_BANKED
voidinterrupt66PIT0_ISR（void）
{
staticcharcount=0;//中断定时计数
PITTF_PTF0=1;//清中断
count++;
LED_Flag++;
g_nSpeedControlPeriod++;
SpeedControlOutput（）;//速度计算输出
g_nDirectionControlPeriod++;
DirectionControlOutput（）;//速度控制
if（count==1）//陀螺仪和加速度计AD采集
{
g_nCarGyroVal=Collect_Gyro（）;//陀螺仪采集10次取的均值
g_nCarAcceVal=Collect_Acce（）;//加速度计采集10次取的均值
tem_angle=AngleCalculate（）;//取得陀螺仪积分后的角度值
}elseif（count==2）
{
g_nLeft=Collect_Left（）;//磁场检测--左
g_nRight=Collect_Right（）;//磁场检测--右
CarAngleAdjust（）;//直立控制电机输出量计算
steand_control=AngleControl（）;//直立控制电机输出量计算
}elseif（count==3）//速度控制
{
g_nSpeedControlCount++;
if（g_nSpeedControlCount>=SPEED_CONTROL_COUNT）{
SpeedControl（）;
g_nSpeedControlCount=0;
g_nSpeedControlPeriod=0;
g_nSpeedCountlOut=speedout（）;
}
DirectionControlOutput（）;//方向控制电机输出计算
g_LeftOut=g_fAngleControlOut-（int）g_fSpeedControlOut-（int）g_fDirectionControl;
g_RightOut=g_fAngleControlOut-（int）g_fSpeedControlOut+（int）g_fDirectionControl;
//电机输出量限幅
if（g_RightOut>MOTOR_OUT_MAX）
g_RightOut=MOTOR_OUT_MAX;
if（g_LeftOut>MOTOR_OUT_MAX）
g_LeftOut=MOTOR_OUT_MAX;
//当小车已经倒下时使其停止运转
if（tem_angle<-30）
g_LeftOut=g_RightOut=0;
if（tem_angle>30）
g_LeftOut=g_RightOut=0;
//电机PWM输出
Set_PWM（g_LeftOut,g_RightOut）;
}
elseif（count==4）//主要要进行方向控制量的计算以及平滑输出计算
{
g_nDirectionControlCount++;
DirectionVoltageSigma（）;
if（g_nDirectionControlCount>=DIRECTION_CONTROL_COUNT）{
DirectionControl（）;
g_nDirectionControlCount=0;
g_nDirectionControlPeriod=0;
}elseif（count==5）
{
Get_Motor_Pulse（）;
count=0;
if（LED_Flag==500）{
LED_Flag=0;
PTJ_PTJ7=~PTJ_PTJ7;}
}
}
/*
***************************************************************
*名称:
陀螺仪角速度积分得到角度
*功能：
*入口参数：
*出口参数：
积分后的角度
*说明：
****************************************************************
*/
signedintAngleCalculate（void）
{
floatfDeltaValue=0;
intCarAcceVal;//加速度处理中间变量
intCarGyroVal;
CarAcceVal=g_nCarAcceVal-GRAVITY_OFFSET;
g_fGravityAngle=CarAcceVal*GRAVITY_ANGLE_RATIO;//加速度处理公式
CarGyroVal=g_nCarGyroVal-GYROSCOPE_OFFSET;
g_fGyroscopeAngleSpeed=CarGyroVal*GYROSCOPE_ANGLE_RATIO;
g_fCarAngle=g_lnCarAngleSigma;
fDeltaValue=（g_fGravityAngle-g_fCarAngle）/GRAVITY_ADJUST_TIME_CONSTANT;g_lnCarAngleSigma+=（float）（g_fGyroscopeAngleSpeed+fDeltaValue）*GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
returng_fCarAngle;
}
/*************直立控制电机输出量计算***************/
intCarAngleAdjust（）
{
staticintnSpeed;
intangle,angle_speed;
angle=AngleCalculate（）;
angle=（int）angle*ANGLE_CONTROL_P;
angle_speed=g_fGyroscopeAngleSpeed*ANGLE_CONTROL_D;
nSpeed=（angle+angle_speed）;
//========================
if（nSpeed>ANGLE_CONTROL_OUT_MAX）
nSpeed=ANGLE_CONTROL_OUT_MAX;
if（nSpeednSpeed=ANGLE_CONTROL_OUT_MIN;
g_fAngleControlOut=nSpeed;
returnnSpeed;
}
/*************方向控制部分*****************/
externintg_nRight,g_nLeft;
//-----------------------------------------------------------------------------------------
voidDirectionVoltageSigma（void）{
intnLeft,nRight;
if（g_nLeft>DIR_LEFT_OFFSET）nLeft=g_nLeft-DIR_LEFT_OFFSET;
elsenLeft=0;
if（g_nRight>DIR_RIGHT_OFFSET）nRight=g_nRight-DIR_RIGHT_OFFSET;
elsenRight=0;
g_fLeftVoltageSigma+=nLeft;
g_fRightVoltageSigma+=nRight;
}
//------------------------------------------------------------------------------
voidDirectionControl（void）{//方向控制
floatfLeftRightAdd,fLeftRightSub,fValue;
intnLeft,nRight;
//--------------------------------------------------------------------------
nLeft=（int）（g_fLeftVoltageSigma/=DIRECTION_CONTROL_COUNT）;
nRight=（int）（g_fRightVoltageSigma/=DIRECTION_CONTROL_COUNT）;
g_fLeftVoltageSigma=0;
g_fRightVoltageSigma=0;
//--------------------------------------------------------------------------
fLeftRightAdd=nLeft+nRight;
fLeftRightSub=nLeft-nRight;
g_fDirectionControlOutOld=g_fDirectionControlOutNew;
if（fLeftRightAddg_fDirectionControlOutNew=0;
}else{
fValue=fLeftRightSub*DIR_CONTROL_P/fLeftRightAdd;
if（fValue>0）fValue+=DIRECTION_CONTROL_DEADVALUE;
if（fValue<0）fValue-=DIRECTION_CONTROL_DEADVALUE;
if（fValue>DIRECTION_CONTROL_OUT_MAX）fValue=DIRECTION_CONTROL_OUT_MAX;
if（fValueg_fDirectionControlOutNew=fValue;
}
}
//方向输出平滑函数
//------------------------------------------------------------------------------
voidDirectionControlOutput（void）{
floatfValue;
fValue=g_fDirectionControlOutNew-g_fDirectionControlOutOld;
g_fDirectionControlOut=fValue*（g_nDirectionControlPeriod+1）/DIRECTION_CONTROL_PERIOD+g_fDirectionControlOutOld;
}
/*************速度控制*************/
externintg_nLeftMotorPeriod,g_nRightMotorPeriod;//左电机转速测定
//------------------------------------------------------------------------------
voidSpeedControl（void）{//速度控制
floatfP,fDelta;
floatfI;
//--------------------------------------------------------------------------
g_fCarSpeed=（g_nLeftMotorPeriod+g_nRightMotorPeriod）/2;
g_nLeftMotorPeriod=g_nRightMotorPeriod=0;
g_fCarSpeed*=CAR_SPEED_CONSTANT;
//-----------------------------------------------------------------------
fDelta=CAR_SPEED_SET-g_fCarSpeed;
//-----------------------------------------------------------------------
fP=fDelta*SPEED_CONTROL_P;
fI=fDelta*SPEED_CONTROL_I;
g_fSpeedControlIntegral+=fI;
//--------------------------------------------------------------------------
if（g_fSpeedControlIntegral>SPEED_CONTROLPI_OUT_MAX）
g_fSpeedControlIntegral=SPEED_CONTROLPI_OUT_MAX;
if（g_fSpeedControlIntegralg_fSpeedControlIntegral=SPEED_CONTROLPI_OUT_MIN;
g_fSpeedControlOutOld=g_fSpeedControlOutNew;
g_fSpeedControlOutNew=fP+g_fSpeedControlIntegral;
}
voidSpeedControlOutput（void）{//速度补偿量应该判断极性
floatfValue;
fValue=g_fSpeedControlOutNew-g_fSpeedControlOutOld;
g_fSpeedControlOut=fValue*（g_nSpeedControlPeriod+1）/SPEED_CONTROL_PERIOD+g_fSpeedControlOutOld;
//对速度补偿进行极性判断便于控制两个方向
if（g_fSpeedControlOut>SPEED_CONTROL_OUT_MAX）
g_fSpeedControlOut=SPEED_CONTROL_OUT_MAX;
if（g_fSpeedControlOutg_fSpeedControlOut=SPEED_CONTROL_OUT_MIN;
}
voidMotorSpeedOut（void）{
floatfLeftVal,fRightVal;
fLeftVal=g_fLeftMotorOut;
fRightVal=g_fRightMotorOut;
if（fLeftVal>0）
fLeftVal+=MOTOR_OUT_DEAD_VAL;
elseif（fLeftVal<0）
fLeftVal-=MOTOR_OUT_DEAD_VAL;
if（fRightVal>0）
fRightVal+=MOTOR_OUT_DEAD_VAL;
elseif（fRightVal<0）
fRightVal-=MOTOR_OUT_DEAD_VAL;
if（fLeftVal>MOTOR_OUT_MAX）fLeftVal=MOTOR_OUT_MAX;
if（fLeftValif（fRightVal>MOTOR_OUT_MAX）fRightVal=MOTOR_OUT_MAX;
if（fRightValSet_PWM（fLeftVal,fRightVal）;
}
/************************************************************/
//输入捕捉通道0中断*/
/*************************************************************/
#pragmaCODE_SEG__NEAR_SEGNON_BANKED
interruptVectorNumber_Vtimch0voidIC0_ISR（void）
{
Count_L++;
TFLG1_C0F=1;
}
#pragmaCODE_SEGDEFAULT
/************************************************************/
//输入捕捉通道1中断*/
/*************************************************************/
#pragmaCODE_SEG__NEAR_SEGNON_BANKED
interruptVectorNumbe
升级会员 