【文章內容簡介】 } return(CRC_Temp)。}void uart_putchar(unsigned char c){ while(!(SCI0SR1amp。0x80)) 。 //keep waiting when not empty SCI0DRL=c。}void OutPut_Data(void){ int temp[4] = {0}。 unsigned int temp1[4] = {0}。 unsigned char databuf[10] = {0}。 unsigned char i。 unsigned short CRC16 = 0。 for(i=0。i4。i++) { temp[i] = (int)OutData[i]。 temp1[i] = (unsigned int)temp[i]。 } for(i=0。i4。i++) { databuf[i*2] = (unsigned char)(temp1[i]%256)。 databuf[i*2+1] = (unsigned char)(temp1[i]/256)。 } CRC16 = CRC_CHECK(databuf,8)。 databuf[8] = CRC16%256。 databuf[9] = CRC16/256。 for(i=0。i10。i++) uart_putchar(databuf[i])。} /*******************************讀取脈沖累加器的數(shù)值*******************************/void GetMotorPulse(void){ float LeftSpeed=0,RightSpeed=0。 LeftSpeed=PACNT。 RightSpeed=PORTB + 255*PORTA_PA3 + 511*PORTA_PA4。 PACNT=0x0000。 PORTA_PA2=1。 PORTA_PA2=0。 if(Judje[0]==0){ LeftSpeed = LeftSpeed。 } if(Judje[1]==0){ RightSpeed = RightSpeed。 } LeftSpeedPulse += LeftSpeed。 RightSpeedPulse += RightSpeed。} /******************************采集加速度計和陀螺儀的值****************************/void Read_adValue(void){ while(!ATD0STAT0_SCF)。 adValue[0] = (float)ATD0DR1。 adValue[1] = (float)ATD0DR2。}/******************************卡爾曼濾波******************************************/void Kalman_Kilter(float angle_m,float gyro_m){ angle += (gyro_m q_bias) * dt。 angle_err = angle_m angle。 Pdot[0] = Q_angle P[0][1] P[1][0]。 Pdot[1] = P[1][1]。 Pdot[2] = P[1][1]。 Pdot[3] = Q_gyro。 P[0][0] += Pdot[0] * dt。 P[0][1] += Pdot[1] * dt。 P[1][0] += Pdot[2] * dt。 P[1][1] += Pdot[3] * dt。 PCt_0 = C_0*P[0][0]。 PCt_1 = C_0*P[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 * P[0][1]。 P[0][0] = K_0*t_0。 P[0][1] = K_0*t_1。 P[1][0] = K_1*t_0。 P[1][1] = K_1*t_1。 angle += K_0 * angle_err。 q_bias += K_1 * angle_err。 angle_dot = gyro_m q_bias。}/******************************角度計算和直立控制**********************************/void AngleCalculate(void){ Read_adValue()。 acceler = adValue[0]ACCELERSET。 gyro = GYROSETadValue[1]。 Kalman_Kilter(acceler,gyro)。 if(acceler=154 ||acceler=300){ PORTA_PA0 = 0。 //電機使能 PORTA_PA1 = 0。 //電機使能 pa_flag=1。 } ZHILI = angle * Angle_P + angle_dot * Angle_D。} void MotorOutPut(void){ float LeftMotorOut,RightMotorOut。 Judje[0]=1。 Judje[1]=1。 LeftMotorOut = ZHILISpeedControlOut+directionout。 RightMotorOut = ZHILISpeedControlOutdirectionout。 if(LeftMotorOut=0){ LeftMotorOut +=5。 } else{ LeftMotorOut =5。 } if(RightMotorOut=0){ RightMotorOut+=5。 } else{ RightMotorOut=5。 } if(LeftMotorOut=250){ LeftMotorOut = } else if(LeftMotorOut=250){ LeftMotorOut = } if(RightMotorOut=250){ RightMotorOut = } else if(RightMotorOut=250){ RightMotorOut = } if(LeftMotorOut0){ Judje[0]=0。 LeftMotorOut=LeftMotorOut。 } if(RightMotorOut0){ Judje[1]=0。 RightMotorOut=RightMotorOut。 } if(Judje[0]==1 amp。amp。 Judje[1]==1){ motor_left_forward((int)LeftMotorOut)。 motor_right_forward((int)RightMotorOut)。 //正轉 } else if(Judje[0]==1 amp。amp。 Judje[1]==0){ motor_left_forward((int)LeftMotorOut)。 motor_right_backward((int)RightMotorOut)。 //左輪正轉，右輪反轉 } else if(Judje[0]==0 amp。amp。 Judje[1]==1){ motor_left_backward((int)LeftMotorOut)。 motor_right_forward((int)RightMotorOut)。 //左輪反轉，右輪正轉 } else if(Judje[0]==0 amp。amp。 Judje[1]==0){ motor_left_backward((int)LeftMotorOut)。 motor_right_backward((int)RightMotorOut)。 //反轉 }} /******************************速度控制輸出函數(shù)************************************/void SpeedControlOutPut(void){ float fValue。 fValue = SpeedControlOutNew SpeedControlOutOld。 SpeedControlOut = fValue * (SpeedControlPeriod+1)/100 + SpeedControlOutOld。}/******************************速度控制函數(shù)****************************************/void SpeedControl(void){ float fDelta。 float fP,fI。 CarSpeed = (LeftSpeedPulse+RightSpeedPulse)*。 LeftSpeedPulse = RightSpeedPulse = 0。 fDelta = CarSpeedSet CarSpeed。 fP = fDelta * SpeedP。 fI = fDelta * SpeedI。 SpeedControlIntegral += fI。 if(SpeedControlIntegral10){ SpeedControlIntegral=10。 } if(SpeedControlIntegral10){ SpeedControlIntegral=10。 } SpeedControlOutOld = SpeedControlOutNew。 SpeedControlOutNew = fP + SpeedControlIntegral。} /***********************************CCD采集數(shù)據(jù)***********************************/void RD_TSL(void) { byte i=0。 int temp_int。 TSL_CLK=1。//起始電平高 TSL_SI=0。 //起始電平低 SamplingDelay()。 //合理的延時 TSL_SI=1。 //上升沿 TSL_CLK=0。//下降沿 SamplingDelay()。 //合理延時 TSL_CLK=1。//上升沿 TSL_SI=0。 //下降沿 SamplingDelay()。 //合理延時 for(i=0。i128。i++) { TSL_CLK=0。//下降沿 SamplingDelay()。 //合理延時 while(!ATD0STAT0_SCF)。//等待轉換結束 temp_int = (int)ATD0DR0。 directionanglevalue=ATD0DR3。 Pixel[i]=(byte)(temp_int4)。 //AD采集 directionanglepluse+=directionanglevalue。 TSL_CLK=1。//上升沿 SamplingDelay()。