leftout

#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();//取得陀螺仪积分后的角度值

}elif(count==2)

{

g_nLeft=Collect_Left();//磁场检测--左

g_nRight=Collect_Right();//磁场检测--右

CarAngleAdjust();//直立控制电机输出量计算

steand_control=AngleControl();//直立控制电机输出量计算

}elif(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);

}

elif(count==4)//主要要进行方向控制量的计算以及平滑输出计算

{

g_nDirectionControlCount++;

DirectionVoltageSigma();

if(g_nDirectionControlCount>=DIRECTION_CONTROL_COUNT){

DirectionControl();

g_nDirectionControlCount=0;

g_nDirectionControlPeriod=0;

}elif(count==5)

{

Get_Motor_Pul();

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;

elnLeft=0;

if(g_nRight>DIR_RIGHT_OFFSET)nRight=g_nRight-DIR_RIGHT_OFFSET;

elnRight=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;

}el{

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;

elif(fLeftVal<0)

fLeftVal-=MOTOR_OUT_DEAD_VAL;

if(fRightVal>0)

fRightVal+=MOTOR_OUT_DEAD_VAL;

elif(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

interruptVectorNumber_Vtimch1voidIC1_ISR(void)

{

Count_R++;

TFLG1_C1F=1;

}

#pragmaCODE_SEGDEFAULT

/*

*******************************************************

*测电机脉冲数

*******************************************************

*/

voidGet_Motor_Pul(){//相当于5ms采集一次速度脉冲,100ms使用一次

if(g_LeftOut>0)

g_nLeftMotorPeriod+=Count_L;

el

g_nLeftMotorPeriod-=Count_L;

if(g_RightOut>0)

g_nRightMotorPeriod+=Count_R;

el

g_nRightMotorPeriod-=Count_R;

Count_L=0;

Count_R=0;

}

/***************电机PWM输出函数****************/

voidSet_PWM(floatfLeftVoltage,floatfRightVoltage)

{

//-----------------------------------------------------------------------

if(fLeftVoltage>0){

PWMDTY01=fLeftVoltage;//左轮正转

PWMDTY23=0;

}

el{

PWMDTY23=-fLeftVoltage;//右轮正转

PWMDTY01=0;

}

//----------------------------------------------------------------------

if(fRightVoltage>0){

PWMDTY67=fRightVoltage;//左轮反转

PWMDTY45=0;

}

el{

PWMDTY45=-fRightVoltage;//右轮反转

PWMDTY67=0;

}

}

/***********串口输出程序***********/

voidSci_Tx(unsignedinttx_date)

{

while(!(SCI0SR1_TC&&SCI0SR1_TDRE));

SCI0DRL=tx_date;//发送数据

}

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

*名称：CRC_CHECK

*功能：实现CRC-16校验

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

unsignedshortCRC_CHECK(unsignedchar*Buf,unsignedcharCRC_CNT)

{

unsignedshortCRC_Temp;

unsignedchari,j;

CRC_Temp=0xffff;

for(i=0;i

CRC_Temp^=Buf[i];//

for(j=0;j<8;j++){

if(CRC_Temp&0x01)

CRC_Temp=(CRC_Temp>>1)^0xa001;

el

CRC_Temp=CRC_Temp>>1;

}

}

return(CRC_Temp);

}

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

//功能：完成数据databuf发送

//入口参数：无

//出口参数：无

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

voidOutPut_Data(void)

{

inttem[4]={0};

unsignedinttemp1[4]={0};

unsignedchardatabuf[10]={0};

unsignedchari;

unsignedshortCRC16=0;

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

for(i=0;i<4;i++)

{

tem[i]=(int)OutData[i];

temp1[i]=(unsignedint)tem[i];

}

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

for(i=0;i<4;i++)

{

databuf[i*2]=(unsignedchar)(temp1[i]%256);

databuf[i*2+1]=(unsignedchar)(temp1[i]/256);

}

CRC16=CRC_CHECK(databuf,8);//在此进行数据检验将返回值给CRC16

databuf[8]=CRC16%256;

databuf[9]=CRC16/256;

for(i=0;i<10;i++)

Sci_Tx(databuf[i]);

}

/***************主要参数宏定义部分**************/

//直立控制相关参数

#defineCAR_ANGLE_SET0//角度给定值

#defineCAR_ANGLE_SPEED_SET0//角速度给定值

#defineCENTER0

#defineANGLE_CONTROL_P130.00//9.65//角度控制P

#defineANGLE_CONTROL_D5.0//0.21

#defineANGLE_CONTROL_OUT_MAX1000//方向控制电机输出最大值

#defineANGLE_CONTROL_OUT_MIN-1000//方向控制电机输出最小值

//------------------------------------------------------------

//滤波相关参数

#defineGRAVITY_OFFSET1810;//重力加速的零偏

#defineGRAVITY_ANGLE_RATIO0.1216//1deg->12

#defineGYROSCOPE_OFFSET1170//陀螺仪的零偏，减小靠前，增大向后

#defineGYROSCOPE_ANGLE_RATIO-0.17//陀螺仪比例系数

#defineGRAVITY_ADJUST_TIME_CONSTANT2.0//重力加速的时间常数

#defineGYROSCOPE_ANGLE_SIGMA_FREQUENCY0.005//积分频率

//-------------------------------------------------------

#defineMOTOR_OUT_MAX1000//电机输出最大值

#defineMOTOR_OUT_MIN-1000//电机输出最小值

#defineMOTOR_OUT_DEAD_VAL1

#defineVOLTAGE_LEFT0

#defineVOLTAGE_RIGHT0

//-----------------------------------------------------------

//方向控制相关参数

#defineDIR_LEFT_OFFSET1255//电磁检测左线圈零偏值

#defineDIR_RIGHT_OFFSET1654//电磁检测右线圈零偏值

#defineCONTROL_PERIOD5//Every5msadjustallcontrol

#defineDIRECTION_CONTROL_PERIOD(DIRECTION_CONTROL_COUNT*CONTROL_PERIOD)

#defineLEFT_RIGHT_MINIMUM2

#defineDIR_CONTROL_P180.0//方向控制P

#defineDIRECTION_CONTROL_DEADVALUE0.0

#defineDIRECTION_CONTROL_OUT_MAX500//方向控制电机输出最大值

#defineDIRECTION_CONTROL_OUT_MIN-500//方向控制电机输出最小值

#defineOPTICAL_ENCODE_CONSTANT100//

#defineSPEED_CONTROL_COUNT20//20ms*5ms速度每100MS调整一次

#defineDIRECTION_CONTROL_COUNT2//方向控制次数方向每10ms调整一次

//--------------------------------------------------------------

//速度控制相关参数

#defineSPEED_CONTROL_PERIOD(SPEED_CONTROL_COUNT*CONTROL_PERIOD)

//#defineCAR_SPEED_CONSTANT1000.0/(SPEED_CONTROL_COUNT*CONTROL_PERIOD)/

(float)OPTICAL_ENCODE_CONSTANT

#defineCAR_SPEED_CONSTANT0.0021

#defineSPEED_CONTROL_P1600.0//0.005//过大补偿值将抑制直立控制

#defineSPEED_CONTROL_I80.0//i参数过大容易达到饱和

#defineSPEED_CONTROLPI_OUT_MAX1000

#defineSPEED_CONTROLPI_OUT_MIN-1000

#defineSPEED_CONTROL_OUT_MAX1000

#defineSPEED_CONTROL_OUT_MIN-1000

#defineCAR_SPEED_SETSpeed_t