#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;

}

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

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

/// 舵机 PID

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

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

int Pcrtl(int ch1,int ch2 )

{

E0=ch1-0; //增量计算

Error0=E0-E1;

Error1=E1-E2;

iIncpid=(int)(Ki*Error0+Kp*E0+Kd*(Error0-Error1));

E1=E0;

E2=E1; //存储误差，用于下次计算

return iIncpid;

}

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

void shache(int sha,int tim)

{

PWMDTY3=sha;

time=tim;

PITCE_PCE1=1;

PITINTE_PINTE1=1; //enable interupt channel 0

zd=0;

}

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

void vIOPortInit(void)

{

DDRA=0X00; //PROT B 键盘输出

PORTA=0X00;

DDRB=0xff;

DDRJ=0xFF; //PROTB

PTJ=0X80;

}

void delayms(int ms) //40MHz--1ms

{

int ii,jj;

if (ms<1) ms=1;

for(ii=0;ii<ms;ii++)

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

}

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

void SetBusCLK_64M(void)

{

CLKSEL=0X00; //disengage PLL to system

PLLCTL_PLLON=1; //turn on PLL

SYNR =0xc0 | 0x07;

REFDV=0xc0 | 0x01;

POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;

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

_asm(nop);

while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;

CLKSEL_PLLSEL =1; //engage PLL to system;

}

void ECT_Init(void) /*ECT初始化，使用输入捕捉功能*/

{

TIOS =0x00; //设为输入捕捉

TSCR1=0x80; //定时器使能

TSCR2=0x07;

TIE=0b00000010; //开中断

TCTL4=0b00001000; //0 ,1触发电平:

//TCTL3=0b10101010;

}

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

void PWMInit(void)

{

PWME=0x00; //禁止PWM输出

PWMPOL=0xff;

PWMCLK=0x08;

PWMCTL=0x10;

PWMSCLB=80;

PWMCAE=0x00; //左对齐

PWMPRCLK=0x44; //PWM时钟CLOCKA，

PWMPER01=40000;

PWMPER3=250;

PWMPER2=255;

PWMDTY2=0;

PWMDTY3=31;

PWMDTY01=4675;

PWME=0x0f; /*使能01,23通道*/

}

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

void SCI(void)

{

SCI1BDH=0X01;

SCI1BDL=0XA0;

SCI1CR1=0X00;

SCI1CR2=0X08;

}

void shacheV(int ch3)

{

PWMDTY3=ch3;

while(FeedBack>30);

PWMDTY3=30;

zd=0;

}

void TERMIO_PutChar(unsigned char ch)

{

while(!(SCI1SR1&0x80)) ; //keep waiting when not empty

SCI1DRL=ch;

}

unsigned char TERMIO_GetChar(void)

{

while(!(SCI1SR1&0x80)) ; //keep waiting when not empty

return SCI1DRL;

}

void un(unsigned char ch)

{

while(!(SCI1SR1&0X40))

{

}

SCI1DRL=ch;

}

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

void ADInit(void)

{

ATD0CTL1=0x00;

ATD0CTL2=0X40; /* 使能AD，清除标志 */

ATD0CTL3=0XB0; /* 队列长度为6，依次存入结果寄存器，继续转换 */

ATD0CTL4=0X05; /* 8位精度，总线频率32分频 */

ATD0CTL5=0X30; /* ，多通道，连续，从AD0开始转换*/

ATD0DIEN=0x00; //禁止数字输入

}

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

void PACN_init(void)

{

PACTL_PAEN=0;

TSCR1=0X80; //快速清除flag：PAOVF PAIF

TSCR2=0X03;

PACNT=0;

PACTL_PAEN=1; //启动脉冲累加器A

PACTL_PAMOD=0; //事件计数方式

PACTL_PEDGE=1; //PT7引脚上的出现上升沿时脉冲累加器计数器加1

TSCR1_TEN =1;

}

void Pit0_Init(void)

{

PITCFLMT_PITE=0; //disable PIT

PITCE_PCE0=1; //enable timer channel 0

PITMUX_PMUX0=0; //ch0 connected to micro timer 0

PITMTLD0=0X0F ; //micro time base 0 equals 255 clock cycles

PITLD0=0XFFFF; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.

//时间计算

//For example, for a 16 MHz bus clock, the maximum time-out period equals:

//15 * 65536 * /64000000 = 0.01536s.

PITINTE_PINTE0=1; //enable interupt channel 0

PITCFLMT_PITE=1; //enable PIT

}

void Pit1_Init(void)

{

PITCFLMT_PITE=0; //disable PIT

//PITCE_PCE1=1; //enable timer channel 0

PITMUX_PMUX1=0; //ch0 connected to micro timer 0

PITMTLD1=0X0F ; //micro time base 0 equals 255 clock cycles

PITLD1=0XFFFF; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.

//时间计算

//For example, for a 16 MHz bus clock, the maximum time-out period equals:

//15 * 65536 * /64000000 = 0.01536s.

//PITINTE_PINTE1=1; //enable interupt channel 0

PITCFLMT_PITE=1; //enable PIT

}

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

void bustor(int x[],int n)

{

int i,j,m,k;

for(i=1;i<=n-1;i++)

{

k=1;

for(j=1;j<=n-i;j++)

if(x[j]>x[j+1])

{

m=x[j];

x[j]=x[j+1];

x[j+1]=m;

k=0;

}

if(k==1)

break;

}

}

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

void kongzhi(void)

{

vIOPortInit() ;

SetBusCLK_64M() ;

ADInit() ;

SCI();

PORTB=0X00;

PWMInit();

PACN_init();

Pit0_Init();

Pit1_Init();

for(;;)

{

ECT_Init();

while(!ATD0STAT0_SCF); /*等待当前队列转换完成*/

b=ATD0DR0L;

c=ATD0DR1L;

d=ATD0DR2L;

e=ATD0DR3L;

f=ATD0DR4L;

g=ATD0DR5L;

//printf("x1=%d x2=%d FeedBack=%d EE0=%d iiIncpid=%d sp=%d b=%d c=%d ",x1,x2,FeedBack,EE0,iiIncpid,sp,b,c) ;

if(b>0x10&&c>0x10)

{

x1=b-210;

x2=c-210;

if(d<e)

x1=-x1;

if(f<g)

x2=-x2;

y=Pcrtl(x1,x2);

if(y>560)

y=560;

if(y<-450)

y=-450;

PWMDTY01=4675-y;

//printf("x1=%d x2=%d",x1,x2);

if(x2>80||x2<-80) //大弯道

{

if(zd==1&&FeedBack>78)

{

sudu(65);

if(d>e)

{

PORTB=0XF0;

shacheV(8);

}

else

{

PORTB=0X0F;

shacheV(52);

}

}

}

else //直道

{

if(x1>50||x1<-50)

{

x4++;

if(x4<60)

sudu(60);

else

sudu(60);

}

else

{

zd=1;

sudu(100);

PWMDTY3=30;

x4=0;

}

}

}

else

sudu(65);

}

}

void main(void) {

/* put your own code here */

EnableInterrupts;

kongzhi();

for(;;) {

_FEED_COP(); /* feeds the dog */

} /* loop forever */

/* please make sure that you never leave main */

}

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

#pragma CODE_SEG __NEAR_SEG NON_BANKED

void interrupt 9 T1CaptureInterrupt(void)

{ TFLG1=0b00000010;

i++;

PORTB=0xff-i;

un(i);

if(i>0x0c)

{

PTJ=0X00;

}

}

#pragma CODE_SEG DEFAULT

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

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

#pragma CODE_SEG __NEAR_SEG NON_BANKED

void interrupt 66 PIT0_ISR(void)

{

PITTF_PTF0=1; //CLER FLAG

FeedBack=PACNT;

PACNT=0;

}

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

#pragma CODE_SEG __NEAR_SEG NON_BANKED

void interrupt 67 PIT1_ISR(void)

{

PITTF_PTF1=1; //CLER FLAG

r++;

if(r==time)

{ r=0;

zd=0;

PORTB=0XFF;

PWMDTY3=30;

PITCE_PCE1=0;

PITINTE_PINTE1=0; //DISable interupt channel 0

}

}