Question

飞思卡尔电磁组

有没有以前电磁组的程序求一个！！！！！！！

Answer 1

#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 = 0; i < 5; i++)
InputVoltageSigma[i] = 0;
return;
}
// InputVoltageCount <<= 4;
for(i = 0; i < 5; i ++)
{
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 = 0; i < 20; i ++)
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 */