*********************************************************************************** 

* 初始化PID參數 

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

void Init_PID() 

{ 

CarState.E0 = 0; 

CarState.E1 = 0; 

CarState.E2 = 0; 

CarState.E3 = 0; 

CarState.Integral = 0; 

} 

/* 

********************************************************************************** 

* 信號處理函數 

* 

* 程序描述: 對傳感器采集過來的數據進行處理，得到一些基本的計算參數 

* 

* 說明： 無 

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

int SignalProcess( unsigned int signal ) 

{ 

const int BitValue[8] = {43,26,12,6,-6,-12,-26,-43}; //MAX:28 

int i,CurrPoint=0,LastPoint=0,BitNum=0; 

unsigned char SignalBit[8]; 

for(i=0;i<8;i++) 

{ 

SignalBit[i] = signal & 0x0001; 

BitNum += SignalBit[i]; 

signal >>= 1; 

} 

switch(BitNum) 

{ 

case 1: 

for(i=0;i<8;i++) 

if(SignalBit[i] != 0) 

CurrPoint += BitValue[i]; 

CarState.E0 = CurrPoint; 

break; 

case 2: 

for(i=0;i<8;i++) 

if(SignalBit[i] != 0) 

CurrPoint += BitValue[i]; 

CurrPoint >>= 1; 

CarState.E0 = CurrPoint; 

break; 

default: 

CarState.E0 = CarState.E1; 

break; 

} 

return CalculateP()*100; 

} 

/* 

********************************************************************************** 

* 

* PID計算函數 

* 

* 程序描述: 計算P參數 

* 

* 說明： 無 

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

int CalculateP(void) 

{ 

CarState.E1 = CarState.E0; 

return((int)CarState.E0); 

} 

/* 

*********************************************************************************** 

* PID計算函數 

* 

* 程序描述: 對傳感器采集過來的數據進行處理，得到一些基本的計算參數 

* 

* 說明： 無 

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

float CalculatePID(void) 

{ 

float P, I = 0, D; 

/* parameter const */ 

float Kp = 1.0, Ki = -0.0002, Kd = -0.0002; 

/* P parameter */ 

P = CarState.E0 * Kp; 

/* I parameter */ 

if(P+I<2) 

{ 

CarState.Integral += Ki * CarState.E0; 

I = CarState.Integral; 

} 

/* D parameter */ 

D = Kd * ( CarState.E0 + 3*CarState.E1 - 3*CarState.E2 - CarState.E3 )/6.0; 

CarState.E3 = CarState.E2; 

CarState.E2 = CarState.E1; 

CarState.E1 = CarState.E0; 

return (P+I+D); 

} 
/* 

*********************************************************************************** 

* Test.c 

* Description: This file includes I/ O function for test, the PWM outputs function for test, function 

* testing sensors. 

* (c) Copyright 2006,Zhao Cheng 

* All Rights Reserved 

* 

* By : Zhao Cheng 

* Note : Don't change this file if possible. 

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

#include <hidef.h> 

#include <mc9s12dg128.h> 

#define HIGHSPEED 8000 

#define LOWSPEED 11000 /* 速度變量，0-24000 數值越大，速度越慢 */ 

void PWMout(int, int); 

/* 24000-20000 */ 

void IOtest(void) 

{ 

static unsigned char i=0,j=0x01,k; 

DDRB = DDRA = 0xFF; 

PORTB = 0xf0; 

for(;;) 

{ 

k=(~j)&0x7f; 

PORTA = PORTB = k; 

while (TCNT != 0x0000); 

while (TCNT == 0x0000) 

{ 

if(i>9) 

{ 

j=j<<1; 

i=0; 

} 

i++; 

} 

if(j>=0x80) 

j=0x01; 

} 

} 

void PWMtest(void) 

{ 

int counter=-4500; 

DDRB = 0xff; 

PORTB = 0xff; 

TSCR1 = 0x80; /* enable timer TCNT */ 

TSCR2 = 0x00; /* TCNT prescaler setup */ 

for(;;) 

{ 

while (TCNT != 0x0000); 

while (TCNT == 0x0000); 

counter=counter+30; 

if(counter >= 3000) 

{ 

counter = 0; 

PWMout(4500, LOWSPEED); 

} 

if(counter == 1500) 

{ 

PWMout(-4500, LOWSPEED); 

} 

PORTB = (char)(counter/100); 

} 

} 

void SignalTest(void) 

{ 

unsigned char signal; 

int Direction, Velocity; 

Direction = 0; 

Velocity = LOWSPEED; 

DDRA = 0x00; 

DDRB = 0xff; 

signal = PORTA; 

PORTB = ~signal; 

switch(signal) 

{ 

case 0x08: /* 0001 1000 */ 

case 0x10: 

Direction = 800; 

Velocity = HIGHSPEED; 

break; 

case 0x04: /* 0010 0100 */ 

case 0x20: 

Direction = 1500; 

Velocity = HIGHSPEED; 

break; 

case 0x02: /* 0100 0010 */ 

case 0x40: 

Direction = 2800; 

Velocity = HIGHSPEED; 

break; 

case 0x01: /* 1000 0001 */ 

case 0x80: 

Direction = 4000; 

Velocity = LOWSPEED; 

break; 

case 0x3c: /* 0011 1100 over start line */ 

case 0xff: /* 1111 1111 over crossing line */ 

case 0x00: /* 0000 0000 go straight not need changed state */ 

default: 

break; 

} 

if(signal > 0x0f) 

Direction = -Direction; 

PWMout(Direction, LOWSPEED); 

} 

/* 

*********************************************************************************** 

* LCD1620.c 

* ICC-AVR application builder : 2006-1-8 21:43:48 

* Target : M8 

* Crystal: 4.0000Mhz 

* 

* Note : Don't change this file if possible. 

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

#define CMD_CLEAR 0x01 

#define CMD_RESET 0x02 

#include <iom8v.h> 

#include <macros.h> 

#define LCD_DATA 0xff 

#define LCD_EN 0x01 //PORTC 0 

#define LCD_RS 0x02 //PORTC 1 

#define LCD_RW 0x04 //PORTC 2 

#define LCD_DATAPORT PORTB 

#define LCD_ENPORT PORTA 

#define LCD_RSPORT PORTA 

#define LCD_RWPORT PORTA 

void lcd_init(void); 

void lcd_write_cmd(unsigned cmd,unsigned data); 

void lcd_setxy(unsigned char x,unsigned char y); 

void lcd_write_string(unsigned char X,unsigned char Y,unsigned char *str); 

void delay_nus(unsigned int n); 

void delay_nms(unsigned int n); 

void lcd_init(void) 

{ 

DDRB |= LCD_DATA; 

DDRA |= LCD_EN | LCD_RS | LCD_RW; 

LCD_RWPORT&=~LCD_RW; 

LCD_DATAPORT=0x30; //控制字規則：5：8bit,4:16x2,3:5x7 

LCD_ENPORT|=LCD_EN; 

delay_nus(1); 

LCD_ENPORT&=~LCD_EN; 

delay_nus(40); 

lcd_write_cmd(0,0x38); //8bit test 

lcd_write_cmd(0,0x0c); //顯示開 

lcd_write_cmd(0,0x01); //顯示清屏 

lcd_write_cmd(0,0x06); //顯示光標移動設置 

} 

void lcd_write_cmd(unsigned cmd,unsigned data) 

{ 

if(cmd==0) 

LCD_RSPORT&=~LCD_RS; 

else 

LCD_RSPORT|=LCD_RS; 

LCD_DATAPORT&=0x00; 

LCD_DATAPORT=data; 

LCD_ENPORT|=LCD_EN; 

delay_nus(10); 

LCD_ENPORT&=~LCD_EN; 

delay_nus(10); 

} 

void lcd_setxy(unsigned char x,unsigned char y) 

{ 

unsigned char addr; 

if(y==0) 

addr=x+0x80; 

else 

addr=x+0xc0; 

lcd_write_cmd(0,addr); 

} 

void lcd_write_string(unsigned char X,unsigned char Y,unsigned char *str) 

{ 

lcd_setxy(X,Y); 

while(*str) 

{ 

lcd_write_cmd(1,*str); 

str++; 

} 

} 

void delay_1us(void) //1us延時函數 

{ 

asm("nop"); 

} 

void delay_nus(unsigned int n) //N us延時函數 

{ 

unsigned int i=0; 

for (i=0;i<n;i++) 

delay_1us(); 

} 

void delay_1ms(void) //1ms延時函數 

{ 

unsigned int i; 

for (i=0;i<1140;i++); 

} 

void delay_nms(unsigned int n) //N ms延時函數 

{ 

unsigned int i=0; 

for (i=0;i<n;i++) 

delay_1ms(); 

} 

//call this routine to initialize all peripherals 

void main(void) 

{ 

lcd_init(); 

while(1) 

{ 

lcd_write_cmd(0,0x01); //清屏 

delay_nms(2); 

lcd_write_string(0,0,"happy new year"); 

delay_nms(100); 

lcd_write_string(0,1,"LCD successful!"); 

delay_nms(100); 

} 

} 

/******************************* 程序結束 *********************************/