飞思卡尔电磁组

#include <hidef.h> /* common defines and macros */

#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 */

void PWM_Init(void) {//初始化

PWMPRCLK_PCKA = 0x2 /*16383000Hz/2=8192000Hz */

PWMPRCLK_PCKB = 0x2/*8192000Hz/4 =2048000Hz */

PWMCTL_CON01 = 0X1 /* 0 &1 成 16位精度PWM工作*/

PWMPOL_PPOL1 = 0x1 /搜碰* 极性为先高后低 与占空比芦亮相同 */

PWMCAE = 0x00 /* operate in left aligned output mode. */

}

void setPWM01(word per,byte start) {

per = (word)(2048000/per)

PWMPER01 = (word)per/* 设置脉世哗谈冲周期*/

PWMDTY01 = (word)(per/2)/* 设置脉冲占空比为50% */

if(start==1)PWME_PWME1 = 1 /* 脉冲发送开始*/

else PWME_PWME1 = 0 /* 脉冲停止发送*/

}

发脉冲时调用 setPWM01（频率，1）

---