飞思卡尔智能车 智能车源代码光电组(有注解)

智能车源代码光电组(有注解)

#include /* common defines and

macros */

#include /* derivative

information */

#include "math.h"

#include "PWM.h"

#include "ATD.h"

#include "LQprintp.h"

#pragma LINK_INFO DERIVATIVE

"mc9s12dg256b"

unsigned int i = 0;

unsigned int j = 0;

unsigned int t = 0;

byte ad_value[13];

uchar data[13];

int sum = 0;

uchar start_flag = 0;

uchar num = 0;

uchar lw=0;

unsigned int per = 65530;

int SPWM = 0;

int L_SPWM = 0;

unsigned int SPmax = 1000;

int MPWM = 0;

uchar current_corrd = 0;

static unsigned int mem_num = 0;

//***********************PID*******************************

static unsigned int Kp=25;

static unsigned int Kp2=60;

static unsigned int Ki=9;

static unsigned int Kd=30;

static unsigned int rKp=100;

static unsigned int rKp2=60;

static unsigned int rKi=0;

static unsigned int rKd=60;

unsigned short E = 5;

unsigned char q = 1;

int Mp = 0;

int Mi = 0;

int Md = 0;

int Mp2 = 0;

int P_Speed = 0;

int L_u[3];

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

//***********************舵机PID变量**********************

static unsigned int s_sKp=35; //直道PID的P值

static unsigned int s_sKp2=0; //直道PID的二阶P值

static unsigned int s_sKi=0; //直道PID的I值 static unsigned int s_sKd=10; //直道PID的D值

//****用于防止PID溢出******

unsigned short s_sE = 5;

unsigned char s_sq = 1;

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

//****分别存放P I D 值*****

int s_sMp = 0;

int s_sMi = 0;

int s_sMd = 0;

int s_sMp2 = 0;

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

int sL_u[3]; //存放前3次理论速度与实际速度的差值

int last_corrd[3][10] ={{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},

{0, 0, 0, 0, 0, 0, 0, 0, 0,

0},

{0, 0, 0, 0, 0, 0, 0, 0, 0,

0}};

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

//*********************PID调试中断**************************

unsigned char cp = 0;

unsigned char ci = 0;

unsigned char cd = 0;

unsigned int search_PACN10;

unsigned int np = 0;

unsigned int sp[500];

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

//*********************存储前20点的数据*********************

int L_num[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int sum_corrd = 0;

int wb = 0; //记录当前状态 黑为0，

白为1；

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

//********************红外滤波*****************************

int corrd[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int T_corrd = 0;

int is_white = 0;

int numb = 0;

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

//***********************数据统计***************************

int corrd_sate[23] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

0};

int SPWM_sate[15] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

0, 0, 0};

int MPWM_sate[11] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uchar corrd_time[512]; uchar SPWM_time[512];

uchar MPWM_time[512];

uchar PACN_time[512];

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

void setbusclock(void)

{

CLKSEL=0X00; //disengage

PLL to system

PLLCTL_PLLON=1; //turn on PLL

SYNR=1;

REFDV=1;

//pllclock=2*osc*(1+SYNR)/(1+REFDV)=32MHz;

_asm(nop); //BUS CLOCK=16M

_asm(nop);

while(!(CRGFLG_LOCK==1)); //when pll

is steady ,then use it;

CLKSEL_PLLSEL =1; //engage

PLL to system;

}

void Dly_ms(int ms)

{

int ii,jj;

if (ms<1) ms=1;

for(ii=0;ii

for(jj=0;jj<2670;jj++);

//busclk:16MHz--1ms

}

static void SCI_Init(void)

{

SCI0CR2=0x2c; //enable Receive Full

Interrupt,RX enable,Tx enable

SCI0BDH=0x00; //出口波特率为9600

SCI0BDL=0x68;

//SCI0BDL=busclk/(16*SCI0BDL)

//busclk 8MHz,

9600bps,SCI0BDL=0x34

//busclk 16MHz,

9600bps,SCI0BDL=0x68

//busclk 24MHz,

9600bps,SCI0BDL=0x9C

} //busclk 32MHz,

9600bps,SCI0BDL=0xD0

static void IOC_Init(void)

{ PERT = 0XFF;

//PPST=0XFF;

DDRT=0XFe;

PBCTL=0X50;//PT0 PIN,PACN10

16BIT,FALLing edge,NOT INTERRUPT

//PACN0=200;

//PACN1=0xFF;

TCTL4=0x01;//40表示ICx禁止, 1表示上升沿,

2表示下降沿, 3表示任何沿

//TIE =0x00;//每一位对应相应通道中断允许,0表示禁止中断 TIOS =0xfe;//每一位对应通道的: 0输入捕捉,1输出比较

}

unsigned int get_Speed()

{

int Speed;

Speed = PACN10;

PACN10 = 0;

return Speed;

}

void s_PID_MPWM()

{

L_u[2] = L_u[1];

L_u[1] = L_u[0];

L_u[0] = P_Speed - search_PACN10;

Mp = Kp * L_u[0];

Mi = Mi + Ki * L_u[0];

Md = Kd * (L_u[0] - 2 * L_u[1] + L_u[2]);

Mp2 = Kp2 * (L_u[0] - L_u[1]);

if(((Ki * L_u[0]) > E)||((Ki * L_u[0]) < -E)) q = 0;

else q = 1;

MPWM = MPWM + Mp + q * Mi + Md + Mp2;

if(MPWM > 1000) MPWM = 1000;

if(MPWM < -1000) MPWM = -1000;

Set_MPWM(MPWM);

}

void r_PID_MPWM()

{

L_u[2] = L_u[1];

L_u[1] = L_u[0];

L_u[0] = P_Speed - search_PACN10;

Mp = rKp * L_u[0];

Mi = Mi + rKi * L_u[0];

Md = rKd * (L_u[0] - 2 * L_u[1] + L_u[2]);

Mp2 = rKp2 * (L_u[0] - L_u[1]);

if(((rKi * L_u[0]) > E)||((rKi * L_u[0]) < -E)) q = 0;

else q = 1;

MPWM = MPWM + Mp + q * Mi + Md + Mp2;

if(MPWM > 1000) MPWM = 1000;

if(MPWM < -1000) MPWM = -1000;

Set_MPWM(MPWM);

}

//*******************舵机PID控制函数*************************

void s_PID_SPWM()

{