第五届飞思卡尔智能车电磁组获奖程序

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

#include "derivative.h" /* derivative-specific definitions */

#include <stdio.h>

#include <MC9S12XS128.h>

#include <stdlib.h>

#include <math.h>

#pragma LINK_INFO DERIVATIVE "mc9s12xs128"

#define K 1

#define N 3

unsigned int i=0,j=0,m=0,n=0;

unsigned char str[]={'0'};

signed int b=0,c=0,d=0,e=0,f=0,g=0,spe=0,r=0,l,x3,y,z,x1,x2,x4;

int time;

int zd=0,flag;

int pp;

/////////////////////////////////////

//// 电机 PID 定义

/////////////////////////////////////

int SetPoint=0; //设定目标 Desired Value

int FeedBack=0;

float KKp=1.5; //比例常数 Proportional Const

float KKi=0;

float KKd=0.6; //微分常数 Derivative Const

signed int EE0=0; //当前误差

signed int EE1=0; //Error[-1]

signed int EE2=0; //Error[-2]

signed int EError0=0,EError1=0;

signed int iiIncpid=0;

int sp=0;

///////////////////////////////

/// 舵机 PID 定义

//////////////////////////////

float Kp=3; //比例常数 Proportional Const

float Ki=1;

float Kd=4.2; //微分常数 Derivative Const

signed int E0=0 ; //当前误差

signed int E1=0; //Error[-1]

signed int E2=0; //Error[-2]

signed int Error0=0,Error1=0;

signed int iIncpid=0;

/////////////////////////////////////

/////////////////////////////////////

//// 电机 PID

/////////////////////////////////////

////////////////////////////////////

unsigned int Getspeed(int cch1,int cch2 )

{

EE0=cch1-cch2; //增量计算

EError0=EE0-EE1;

EError1=EE1-EE2;

iiIncpid=(int)(KKi*EError0+KKp*EE0+KKd*(EError0-EError1));

EE1=EE0;

EE2=EE1; //存储误差，用于下次计算

sp+=iiIncpid;

if(sp>210)

sp=210;

if(sp<0)

sp=0;

return sp;

}

void sudu(int SetPoint)

{

pp=Getspeed(SetPoint,FeedBack);

PWMDTY2=pp;

}