程序.docx
- 文档编号:5422995
- 上传时间:2022-12-16
- 格式:DOCX
- 页数:16
- 大小:20.63KB
程序.docx
《程序.docx》由会员分享,可在线阅读,更多相关《程序.docx(16页珍藏版)》请在冰豆网上搜索。
程序
#include
#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(nSpeed nSpeed=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(fLeftRightAdd g_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(fValue g_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_fSpeedControlIntegral g_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_fSpeedControlOut g_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(fLeftVal if(fRightVal>MOTOR_OUT_MAX)fRightVal=MOTOR_OUT_MAX; if(fRightVal Set_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
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 程序