#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include <math.h>
typedef char int8;
#define MAXCOUNTER 1500
#define WBUFSIZE 10
#define INVALIDSENSOR 1000
long counter=0x00000000; // for out of course
int blackcount = 0; // for cross courses
unsigned int sensor=0;
int delta=0, delta0=0; // 舵机偏转角（0.1 度）
int w[WBUFSIZE] = {0,0,0,0,0,0,0,0,0,0}; int w_p=0; // 偏差（mm）缓冲器及其指针
int w1=0, w2=0, w3=0;// 平均偏差（mm）
int DT = 1; // time interval (ms)
int PulseNumAll=0; // 总脉冲数
int u; // longitude speed(mm/s)
//int psi_r0=0, psi_r1=0; // psi requested(degree)
int H=100, L=200; // H是传感器到前轮距离。L是前后轮距离
int run_state=0; // state of the car
int boma;
int c=0;
int c1=0;
int c2=0;
int c3=0;
int decv=0;
/*******************PLL部分***************************/
void SetBusCLK_64M(void) //OSC=16MHz
{
CLKSEL=0X00; //disengage PLL to system
PLLCTL_PLLON=1; //turn on PLL
SYNR =0xc0 | 0x07;
REFDV=0x80 | 0x01;
POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz;
//BUS CLOCK=pllclock/2=64MHz
// fVCO= 2*fOSC*(SYNDIV + 1)/(REFDIV + 1)
// fPLL= fVCO/(2 × POSTDIV)
// fBUS= fPLL/2
// If POSTDIV = $00 then fPLL is identical to fVCO (divide by one).
_asm(nop);
_asm(nop);
while(!(CRGFLG_LOCK==1)); //时钟校正同步，when pll is steady ,then use it;
CLKSEL_PLLSEL =1; //engage PLL to system;
}
/*************************PWM,I/O初始化*******************************/
void PWM_Init(void)
{
PWME=0x00; //PWM disable
PWMPOL=0xFF; //输出开始为高电平, 当给定计数到来时变低电平
PWMCLK=0x00; //PWM0,1和PWM4,5时钟源选择ClockA; PWM2,3时钟源选择ClockB;
PWMPRCLK=0x66; //ClockA=ClockB=64MHz/64=1MHz
PWMCAE=0x00; //对齐方式：左对齐
//PWMCTL_CON01=1; //PWM01 合并 16 bit
PWMPER0=200; //PWM1 Period=200/1M=0.2ms, f=5kHZ
PWMDTY0=200; //占空比=[(PWMDTYx+1)/(PWMPERx+1)] ×100%
PWME_PWME0 = 1; //电机enable
PWME_PWME1=0;
PWMCTL_CON45=1; //PWM45 合并 16 bit
PWMPER45=20000; //PWM5 Period=20000/1M=20ms. f=50HZ
PWMDTY45=1490; //占空比=50%
PWME_PWME5=1; //舵机enable
}
/************数字端口初始化*************/
void Port_Init(void)
{
DDRA = 0X00; //方向控制寄存器，设置A口输入 DDR寄存器置0为输入，置1为输出
PORTA= 0X00; //数据寄存器，初始化时
设置为全0
DDRB = 0X00; //同上
PORTB= 0X00; //
/////////////////////////////////////////////////////////////////
DDRT=0x00;
boma=PTIT&0x0f;
}
// TIM Input capture initialize
void PT4_Capture_Init(void)
{
TSCR1=0x80; //定时器使能
TIE =0x00; //每一位对应相应通道中断禁止,0表示禁止中断
TCTL3=0x02; //EDGnB EDGnA 1表示上升沿, 2表示下降沿, 3表示任何沿
//PT4下降沿有效
TIOS =0x00; //每一位对应通道的: 0输入捕捉,1输出比较
TIE=0xff; //中断使能
}
//PIT initialize, DT ms interrupt
void PitInit(void)
{
PITCFLMT_PITE=0; //disable PIT
PITCE_PCE0=1; //enable timer channel 0
PITMTLD0=64-1; //1M Hz
PITMUX=0X00; // ch0 connected to micro timer 0
PITLD0=DT*1000-1; //INTVERAL micro time bases (PITMTLD+1)*(PITLD+1)/fBUS
PITINTE_PINTE0=1; //enable interupt channel 0
PITCFLMT_PITE=1; //enable PIT
//PITCNT0,类似于自由记数器,只不过一直是递减,TCNT是一直递增
}
// 控制马达速度
void set_speed(void) {
if (counter<MAXCOUNTER) {
/////////////////速度选择///////////////////
switch(boma) {
case 0x00:
PWMDTY0 = 120;break;
case 0x01:
PWMDTY0=110+delta/30*delta/100;break;
case 0x02:
PWMDTY0=100+delta/30*delta/100;break;
case 0x04:
PWMDTY0=90+delta/30*delta/100;break ;
case 0x08:
PWMDTY0=80+delta/30*delta/100;break;
case 0x09:
PWMDTY0=80;break;
case 0x03:
PWMDTY0=70+delta/30*delta/100;break;
case 0x07:
PWMDTY0=60+delta/30*delta/100;break;
case 0x0e:
PWMDTY0=50+delta/30*delta/100;break;
case 0x0f:
PWMDTY0=40+delta/30*delta/100;break;
default:
break;
}
////////////////////////////////////////////////
/* if(boma) PWMDTY0 = 90;
else PWMDTY0=50+delta/30*delta/100;*/
}
else
{
PWMDTY0=200;
}
}
// 读传感器值
void get_sensorvalue(void)
{
sensor=PORTA | ((PORTB&0X1F)<<8);
sensor=~sensor;
sensor=sensor&0x1fff;
}
// 传感器值转换为偏差W (mm)
// x x xxxxXxxxx x x
// 2 1 098765432 1 0
int sensor_to_w(void) {
int w = INVALIDSENSOR+1; // invalid value
switch(sensor) {
case 0x0001: // bit 0
w = 80; break;
case 0x0003: // bit 0 1
w = 70; break;
case 0x0002: // bit 1
w = 60; break;
case 0x0006: // bit 1 2
w = 50; break;
case 0x0004: // bit 2
w = 40; break;
case 0x000c: // bit 2 3
w = 35; break;
case 0x0008: // bit 3
case 0x001c: // bit 2 3 4
w = 30; break;
case 0x0018: // bit 3 4
w = 25; break;
case 0x0010: // bit 4
case 0x0038: //
bit 3 4 5
w = 20; break;
case 0x0030: // bit 4 5
w = 15; break;
case 0x0020: // bit 5
case 0x0070: // bit 4 5 6
w = 10; break;
case 0x0060: // bit 5 6
w = 5; break;
case 0x0040: // bit 6
case 0x00e0: // bit 5 6 7
w = 0; break;
case 0x00c0: // bit 6 7
w = -5; break;
case 0x0080: // bit 7
case 0x01c0: // bit 6 7 8
w = -10; break;
case 0x0180: // bit 7 8
w = -15; break;
case 0x0100: // bit 8
case 0x0380: // bit 7 8 9
w = -20; break;
cas
e 0x0300: // bit 8 9
w = -25; break;
case 0x0200: // bit 9
case 0x0700: // bit 8 9 10
w = -30; break;
case 0x0600: // bit 9 10
w = -35; break;
case 0x0400: // bit 10
w = -40; break;
case 0x0c00: // bit 10 11
w = -50; break;
case 0x0800: // bit 11
w = -60; break;
case 0x1800: // bit 11 12
w = -70; break;
case 0x1000: // bit 12
w = -80; break;
default:
break;
} // switch
/*if(sensor==0x1fff) {
counter=0x00000000;
blackcount = 10;
} */
if ((sensor|~0x01f0)==0xffff) //判断三角标志
{
blackcount=10;
++ c1;
if(c1>30&&c2==0)
{ // triangle
counter=0;
c1=0;
c2=c2+1;
}
if(c1>30&&c2>0)
{
counter=0;
}
}
else
{
c1 = 0;
}
if (blackcount>0) --blackcount;
if(counter<MAXCOUNTER)
{
if (w<INVALIDSENSOR) counter=0x00000000;
if((sensor&0x1800) && (sensor&0x0003) // Two ends is black
&& ~(sensor|0x1803) && !blackcount) // There are whites in center
{ //counter=0x00000000;
c3++;
if(c3>5 && c==0) // 判断是起点，前进
{
c=10;
c3=0;
counter=0;
}
//if(c3>2 && c2>0)
if(c3>2 && (c2>0||c>0)) //判断是终点，停止
{
counter += MAXCOUNTER/(10000/DT/u);
}
}
else
{
c3=0;
++ counter;
}
}
return w;
}
// 根据偏差平均值计算转向角
void set_delta(void) { // 20090710
int a;
a = (int)((long)L*w1*2*573/((long)H*H+(long)L*H*2+(long)w1*w1)); // potential delta
if(w1<30 && w1>-30 && delta<150 && delta>-150) run_state=0;
switch(run_state) {
case 0: // beeline
if(w1>=40 && w1>=w2 && w2>=w3) run_state=1;
else if(w1<=-40 && w1<=w2 && w2<=w3) run_state=-1;
break;
case 1: // in cycle 右转
if(w1<=35 && w1<=w2 && w2<=w3 && delta<200) run_state=0;
else {
if(w1<w2) a = delta+0.004*573*(w1-w2)*(10+u/2000)/10; //转角过度
else if(w1>w2 && w2>w3) a = delta+0.004*573*(w1-w2); // 转角不足
// else a=delta;
}
break;
case -1: // in cycle
if(w1>=-35 && w1>=w2 && w2<=w3 && delta>-200) run_stat
e=0;
else {
if(w1>w2) a = delta+0.004*573*(w1-w2)*(10+u/2000)/10;
else if(w1<w2 && w2<w3) a = delta+0.0035*573*(w1-w2);
// else a=delta;
}
break;
default:
break;
}
delta0 = delta;
delta = a;
}
// 根据转向角控制舵机的PWM
void turn(void){
int c1=0, c2=0, k1=10, k2=9; // 20090710
if(delta>0)
PWMDTY45=(int)(1490+delta/10*k1+c1);
else if(delta<0)
PWMDTY45=(int)(1490+delta/10*k2-c2);
else
PWMDTY45=1490;
}
// TIM4 input capture interrupt
void interrupt 12 PT4_Capture(void) {
PulseNumAll++; //输入脉冲计数
TFLG1=0X10; //清中断标志位
}
// 定
时器0中断
void interrupt 66 PIT0(void) {
// read course sensor and speed
get_sensorvalue();
w[w_p]= sensor_to_w();
if(++w_p>=WBUFSIZE) {
w_p = 0;
// speed (mm/s) =
// (pulse count)/(pulse per round)*(wheel girth(mm))*(gear ratio)/(interval(ms))*1000
u = (int)((float)PulseNumAll/8.0*173.0*(18.0/76.0)/(DT*WBUFSIZE)*1000);
PulseNumAll=0;
}
PITTF_PTF0=1; // clear Time-out Flag Bits
}
// 延时函数
void delay(unsigned int time){
static int delay_i;
for(;time>0;time--){
for(delay_i=0;delay_i<10680;delay_i++){
}
}
}
void main(void) {
/* put your own code here */
int8 i,n;
int s1, s2;
SetBusCLK_64M();
PWM_Init() ;
Port_Init();
PT4_Capture_Init();
PitInit();
EnableInterrupts;
delta = 0;
turn();
delay(500);
delta = -400;
turn();
delay(1000);
delta = 400;
turn();
delay(1000);
delta = 0;
turn();
delay(1000);
set_speed(); // 20090710
//delay(300);
counter=0x00000000;
for(;;) {
_FEED_COP(); /* feeds the dog */
if(!w_p) {
set_speed(); // 20090710
// 计算w的平均值
s1 = 0; n = 0;
for(i=0;i<WBUFSIZE;++i) {
if(w[i]<INVALIDSENSOR) { // if sensor value valid
s1 += w[i]; ++n;
}
}
if(!n) continue; // no valid data
s1/=n;
// 剔除无效数据，再次计算w的平均值，保存在w1中
s2 = 0; n = 0;
for(i=0;i<WBUFSIZE;++i) {
if(w[i]>s1-12 && w[i]<s1+12 ) { // if sensor value valid
s2 += w[i]; ++n;
}
}
if(!n) continue; // no valid data
s2/=n;
w3 = w2; // 20090710
w2 = w1;
w1 = s2;
set_delta();
turn();
} // if(w_p)
} /* loop forever */
/* please make sure that you never leave main */
}