#include "derivative.h" /* derivative-specific definitions */
#include <mc9s12xs128.h>
#include "KEY.H"
#include "PLL.H"
#include "PWM.H"
#include "ATD.H"
#include "LCD5110.H"
#include "PIT.H"
#include "MATH.H"
#include "ECT.H"
#define AD_NUM 5
/***************************************************************/
signed int ad_value[AD_NUM]={0,0,0,0,0}
/**********************卡尔曼滤波*****************************************/
float gyro=0,acceler=0,Vref=2.048,jiaodu_jifen=0,jiaodu_error=0//2.048
float angle=0, angle_dot=0
long Tg=3
unsigned int fangdabeishu =9//15
float C_0 = 1
float Q_angle=0.001
float Q_gyro=0.003
float R_angle=0.5
float dt=0.006
float P[2][2] = {{ 1, 0 },
{ 0, 1 }
}
float Pdot[4] ={0,0,0,0}
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1
/*********************中断变量**************************************/
unsigned char
EventCount=0,SpeedControlPeriod=0,DirectionControlPeriod=0,InputVoltageCount=0,SpeedCont
rolCount=0,DirectionControlCount=0,BMQ_count=0
long MotorOut=0
long LeftMotorPulseSigma=0,RightMotorPulseSigma=0,InputVoltageSigma[5]=0
/************************直立PID************************************/
unsigned int JIAODU_CENTER=820,JIAO_SPEED_CENTER=580
unsigned int ZHILI_PID_P=460,ZHILI_PID_D=9,ZHILI_PID_I=25
全国大学生智能汽车邀请赛技术报告
38
float ZHILI_PD=0,ZHILI_I=0,ZHILI=0,PIANJIAO=0
/*************************速度PID*************************************/
unsigned int Sudu_PID_P=0,Sudu_PID_I=0,Sudu_PID_D=0
signed int
SpeedLeft_now=0,SpeedRight_now=0,Speed_want=0,speed[6]={140,20,140,20,140,18}
int Speed_Break_Flag1=0,Speed_Break_Flag2=0,Speed_Break_Count=0
long Speed_P=0,Speed_I=0,Speed_D=0,Speed_error=0,Speed_error_old=0,Speed_error_error=0
float Speed=0,SpeedControlOutOld=0,fValue=0,CarSpeed=0,CarSpeed1=0,CarSpeed2=0
float SpeedControlOut=0
/*************************方向PID******************************/
signed int Turn_PID_P=0,PP[3]={15,30,0},Turn_PID_D=50
signed int LeftVoltageSigma=0,RightVoltageSigma =0
long DirectionControlOutOld=0,DirectionControlOutNew=0
long DirectionControlOut=0,DirectionControlOut1=0
long LeftMotorOut=0,RightMotorOut=0
float fLeftRightSub_old=0, fLeftRightSub=0
int nLeft=0, nRight=0
float DValue=0
/*************************信号PID********************************/
#include "LCDKEY.H" //液晶控制程序
/***********************读AD******************************************/
void Read_Atd_Several()
{
ATD0CTL5=(atd0_scan<<5) + (atd0_mult<<4) + atd0_cd_cc_cb_ca
while(!ATD0STAT0_SCF)
ad_value[0]=ATD0DR0
ad_value[1]=ATD0DR1
ad_value[2]=ATD0DR2
ad_value[3]=ATD0DR3
ad_value[4]=ATD0DR4
}
/************************* 卡尔曼滤波
附录 A
39
*********************************************/
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt//先验估计
angle_err = angle_m - angle//zk-先验估计
Pdot[0]=Q_angle - P[0][1] - P[1][0]// Pk-' 先验估计误差协方差的微分
Pdot[1]=- P[1][1]
Pdot[2]=- P[1][1]
Pdot[3]=Q_gyro
P[0][0] += Pdot[0] * dt// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
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 ZHILI_lvbo(void)
{
angle+=(gyro+ jiaodu_error)/200
全国大学生智能汽车邀请赛技术报告
40
jiaodu_error= (acceler-angle)/Tg
if(angle>5) angle=5
else if(angle<-5) angle=-5
}
/***************************AD计算********************************************/
void AD_calculate(void)
{
acceler=ad_value[0]
gyro=ad_value[1]
gyro=(JIAO_SPEED_CENTER-gyro)*Vref/(1024*0.00067*fangdabeishu)
acceler=(acceler-JIAODU_CENTER )*Vref/(1024*0.8)*60
Kalman_Filter(acceler,gyro)//卡尔曼滤波
//angle_dot=gyro
//ZHILI_lvbo()
}
/************************* 直立PID 控制
*********************************************/
void zhili_kongzhi(void)
{
ZHILI_PD=(0-angle-(PIANJIAO/100))*ZHILI_PID_P+(0-angle_dot)*ZHILI_PID_D
ZHILI_I+=(0-angle-(PIANJIAO/100))*ZHILI_PID_I
if(ZHILI_I>1000) ZHILI_I=1000
if(ZHILI_I<-1000) ZHILI_I=-1000
ZHILI=ZHILI_PD/10+ZHILI_I/10
}
/***************************** 速度PID 控制
*****************************************************/
void SampleInputVoltage(void)
{
Read_Atd_Several()
InputVoltageSigma[0] += ad_value[0]
InputVoltageSigma[1] += ad_value[1]
InputVoltageSigma[2] += ad_value[2]
附录 A
41
InputVoltageSigma[3] += ad_value[3]
InputVoltageSigma[4] += ad_value[4]
InputVoltageCount ++
}
void GetInputVoltageAverage(void)
{
int i
if(InputVoltageCount == 0)
{
for(i = 0i <5i++)
InputVoltageSigma[i] = 0
return
}
// InputVoltageCount <<= 4
for(i = 0i <5i ++)
{
ad_value[i] = (int)(InputVoltageSigma[i] / InputVoltageCount)
InputVoltageSigma[i] = 0
}
InputVoltageCount = 0
}
void DirectionVoltageSigma(void)
{
LeftVoltageSigma += ad_value[2]
RightVoltageSigma += ad_value[3]
}
/*************************方向PID****************/
void DirectionControl(void)
{
nLeft = (int)(LeftVoltageSigma /= 2)
nRight = (int)(RightVoltageSigma /= 2)
LeftVoltageSigma = 0
RightVoltageSigma = 0
全国大学生智能汽车邀请赛技术报告
42
fLeftRightSub_old=fLeftRightSub
fLeftRightSub = nLeft - nRight
DirectionControlOutOld = DirectionControlOutNew
Turn_PID_P=PP[0]
if(ad_value[2]+ad_value[3]<300) Turn_PID_P=PP[1]
DValue = (fLeftRightSub *
Turn_PID_P/100)+((fLeftRightSub-fLeftRightSub_old)*Turn_PID_D/100)
DirectionControlOutNew = DValue
}
void DirectionControlOutput(void)
{
float Value
Value = DirectionControlOutNew - DirectionControlOutOld
DirectionControlOut = Value * (DirectionControlPeriod + 1) / 10 + DirectionControlOutOld
}
/*void DirectionControlOutput(void)
{
DirectionControlOutOld = DirectionControlOut
DirectionControlOut = (float)((ad_value[2] - ad_value[3]) * Turn_PID_P/100)
} */
void GetLeftMotorPulse(void)
{
SpeedLeft_now=PACNT
PACNT=0
附录 A
43
if(LeftMotorOut<0) SpeedLeft_now=-SpeedLeft_now
LeftMotorPulseSigma+=SpeedLeft_now
}
void GetRightMotorPulse(void)
{
SpeedRight_now=PACNT
PACNT=0
if(RightMotorOut<0) SpeedRight_now=-SpeedRight_now
RightMotorPulseSigma+=SpeedRight_now
}
void SpeedControl(void)
{
CarSpeed=(LeftMotorPulseSigma+RightMotorPulseSigma)/10
LeftMotorPulseSigma=0
RightMotorPulseSigma=0
Speed_error_old=Speed_error
Speed_want=speed[0]
Speed_error=Speed_want-CarSpeed
Speed_error_error=Speed_error-Speed_error_old
Speed_P=Speed_error*Sudu_PID_P
Speed_D=Speed_error_error*Sudu_PID_D
Speed_I+=Speed_error*Sudu_PID_I/10
/*if(Speed_error>5||Speed_error<-5)
{
Speed_I=0
} */
SpeedControlOutOld=Speed
Speed=Speed_P+Speed_I+Speed_D
}
void SpeedControlOutput(void)
{
fValue = Speed - SpeedControlOutOld
SpeedControlOut = fValue * (SpeedControlPeriod + 1) / 100 + SpeedControlOutOld
全国大学生智能汽车邀请赛技术报告
44
}
void PWM_out(void)
{
if(LeftMotorOut>0)
{
PWMDTY7=LeftMotorOut+5
PWMDTY5=0//左电机
}
else
{
PWMDTY5=-LeftMotorOut+5
PWMDTY7=0//左电机
}
if(RightMotorOut>0)
{
PWMDTY1=RightMotorOut+5//右电机
PWMDTY3=0
}
else
{
PWMDTY1=0
PWMDTY3=-RightMotorOut+5
}
if(angle>=10||angle<=-10)
{
PWMDTY1=PWMDTY3=PWMDTY5=PWMDTY7=0
}
}
void MotorOutput(void)
{
int Speed_left=0,Speed_right=0
Speed_left=(int)(ZHILI-SpeedControlOut+DirectionControlOut)
Speed_right=(int)(ZHILI -SpeedControlOut-DirectionControlOut)
if(Speed_left >250)Speed_left =250
else if(Speed_left <-250)Speed_left = -250
if(Speed_right >250)Speed_right =250
else if(Speed_right <-250)Speed_right = -250
附录 A
45
LeftMotorOut = Speed_left
RightMotorOut = Speed_right
PWM_out()
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 Pit0_interrupt(void)
{
DisableInterrupts
EventCount++
SpeedControlPeriod++
SpeedControlOutput()
DirectionControlPeriod++
DirectionControlOutput()
if(EventCount >=5)
{ // Motor speed adjust
EventCount = 0// Clear the event counter
BMQ_count++// Motor speed adjust
if(BMQ_count==1)
{
PWMDTY2=255 // Clear the event counter
GetLeftMotorPulse()
PWMDTY0=0
}
else if(BMQ_count==2)
{
BMQ_count=0
PWMDTY0=255
GetRightMotorPulse()
PWMDTY2=0
}
}
else if(EventCount == 1)
{
int i=0
for(i = 0i <20i ++)
SampleInputVoltage()
}
else if(EventCount == 2)
全国大学生智能汽车邀请赛技术报告
46
{
GetInputVoltageAverage()
AD_calculate()
zhili_kongzhi()
MotorOutput()
}
else if(EventCount == 3)
{ // Car speed adjust
SpeedControlCount ++
if(SpeedControlCount >= 20)
{
SpeedControl()
SpeedControlCount = 0
SpeedControlPeriod = 0
}
}
else if(EventCount == 4)
{ // Car direction control
DirectionControlCount ++
DirectionVoltageSigma()
if(DirectionControlCount >= 2)
{
DirectionControl()
DirectionControlCount = 0
DirectionControlPeriod = 0
}
// DirectionControlOutput()
}
PITTF_PTF0=1
EnableInterrupts
}
#pragma CODE_SEG DEFAULT
/********************************************************/
/*************************主函数*************************/
void Init_ALL()
{
Pll_Init()
附录 A
47
Pwm0_Init()
Pwm1_Init()
Pwm2_Init()
Pwm3_Init()
Pwm5_Init()
Pwm7_Init()
ATD0_Init()
Pit_Init()
Ect7_Init()
//Pulse_Add_Init()
init_key()
DDRT=0X7F//液晶使能
LCD_init()
LCD_clear()
PTT_PTT5=1//液晶背光
angle=(JIAODU_CENTER-ad_value[0] )*Vref/(1024*0.8)*90
}
void main(void)
{
DisableInterrupts
Init_ALL()
EnableInterrupts
for()
{
LCD_work()
}
/* please make sure that you never leave main */
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）