?? robbot._c
字號:
//***************************FileName:Robbot.c***********************//
//***************************ICCAVR6.30編譯**************************//
#include <io8535v.h>
#include <macros.h>
//****************************宏定義********************************//
#define Aa 0.5 //PID參數(shù)
#define Ba -0.5 //PID參數(shù)
#define Ca 0 //PID參數(shù)
#define Ab 0.09 //PID參數(shù)
#define Bb -0.1 //PID參數(shù)
#define Cb 0 //PID參數(shù)
#define U1 12 //PID參數(shù)
#define MaxSpeed 0x40 //最大速度
#define MidSpeed 0x18 //中速,用于轉彎
#define Size 6 //任務數(shù)組大小
//*****************************全局變量定義**************************//
char sflag=0x00; //記錄上一次校偏狀態(tài)
char crossflag = 0; //過線標志，用于判斷是否過線
char forflag=0; //記錄上一次機器人行進狀態(tài)
char forlight; //記錄上一次A口光電傳感器的狀態(tài)
float EkA; //本次左邊電機速度誤差
float EkA_1=0; //上次左邊電機速度誤差
float EkA_2=0; //上上次左邊電機速度誤差
float EkB; //本次左邊電機速度誤差
float EkB_1=0; //上次左邊電機速度誤差
float EkB_2=0; //上上次左邊電機速度誤差
char flage=0;
char a=0; //溢出次數(shù)，控制PID窗口時間
char c=0; //控制尋線頻率
int desireV=10;
char b=0;
char time=0; //機器人行走步數(shù)
char fob=0; //=1后退，=0前進
char Task[Size]={0x37,0x27,0x1B,0x29,0x23,0x1B};//任務數(shù)據(jù)數(shù)組
//******************************定時器1初始化*************************//
void Timer1Init(int temptimsk,int temptccrA,int temptccrB)
{unsigned char sreg;
TIMSK = temptimsk;
sreg = SREG; //保存全局中斷標志
_CLI(); //屏蔽所有中斷
TCCR1A = temptccrA;
TCCR1B = temptccrB;
SREG = sreg; //恢復全局中斷標志
}
//******************************寫OCR1A寄存器**************************//
void SetOutputComReg1A(int tempocr)
{unsigned char sreg;
sreg = SREG;
_CLI();
OCR1A = tempocr;
SREG = sreg;
}
//******************************寫OCR1B寄存器**************************//
void SetOutputComReg1B(int tempocr)
{unsigned char sreg;
sreg = SREG;
_CLI();
OCR1B = tempocr;
SREG = sreg;
}
//******************************讀OCR1A寄存器**************************//
int GetOutputComReg1A()
{int temp;
temp = OCR1A;
return(temp);
}
//******************************讀OCR1B寄存器**************************//
int GetOutputComReg1B()
{int temp;
temp = OCR1B;
return(temp);
}
//******************************長延時函數(shù)***************************//
void DELAY(int delaytime)
{int i,j;
for(i=0;i<=delaytime;i++)
{for (j=0;j<=0xFFFE;j++) ;}
}
//******************************短延時函數(shù)***************************//
void delay(int i)
{int j;
for(j=0;j<=i;j++) ;
}
//******************************PID調(diào)節(jié):左電機***********************//
void PIDA()
{char y; //本次采樣速度值
float u; //電壓差值
int z; //本次輸出增量
int temp1; //臨時記錄值
y = TCNT1;
EkA=y-desireV;
if (EkA==(0-desireV)) //當Ek大于某一值時直接加最大
{temp1=0x0000;
SetOutputComReg1A(temp1);}
else
{EkA=Aa*EkA;
EkA_1=Ba*EkA_1;
EkA_2=Ca*EkA_2;
u=EkA+EkA_1+EkA_2;
z=u/U1*0x03FF;
temp1=GetOutputComReg1A();
temp1=temp1+z;
SetOutputComReg1A(temp1);
EkA_2=EkA_1;
EkA_1=EkA;}
TCNT1=0x00;
}
//******************************PID調(diào)節(jié):右電機***********************//
void PIDB()
{char y;
float u; //電壓差值
int z;
int temp1;
y = TCNT2;
EkB=y-desireV;
if(EkB==(0-desireV)) //當Ek大于某一值時直接加最大//
{temp1=0x0000;
SetOutputComReg1B(temp1);}
else
{EkB =Ab*EkB;
EkB_1=Bb*EkB_1;
EkB_2=Cb*EkB_2;
u=EkB+EkB_1+EkB_2;
z=u/U1*0x03FF;
temp1=GetOutputComReg1B();
temp1=temp1+z;
SetOutputComReg1B(temp1);
EkB_2=EkB_1;
EkB_1=EkB;}
TCNT2=0x00;
}
//***************************尋線處理函數(shù)1***************************//
char Talone()
{if ((forlight&0x09)==0) //3單獨亮
return(0);
else
return(1); //返回為校偏狀態(tài)值
}
//***************************尋線處理函數(shù)2***************************//
char FUalone() //4單獨亮
{if ((forlight&0x05)==0)
return(0);
else
return(2);
}
//***************************尋線處理函數(shù)3***************************//
char FIalone() //5單獨亮
{if ((forlight&0x22)==0)
return(0);
else
return(1);
}
//***************************尋線處理函數(shù)4***************************//
char Salone() //6單獨亮
{if ((forlight&0x12)==0)
return(0);
else
return(2);
}
//***************************尋線處理函數(shù)5****************************//
char InLine1(void) //線上偏離判斷
{char discrepancy; //行進狀態(tài)
char l2; //傳感器狀態(tài)
l2=0xFF^PINA;
l2=0x3C&l2;
l2=l2>>2;
switch(l2)
{case 0x00: discrepancy=0; break; //0000b未偏
case 0x01: discrepancy=0; break;
case 0x02: discrepancy=0; break;
case 0x04: discrepancy=2; break; //0010b左偏
case 0x05: discrepancy=2; break;
case 0x06: discrepancy=2; break;
case 0x08: discrepancy=1; break; //0001b右偏
case 0x09: discrepancy=1; break;
case 0x0A: discrepancy=1; break;}
return(discrepancy);
}
//***************************尋線處理函數(shù)6****************************//
char InLine2(void) //線上偏離判斷
{char discrepancy;
char l2;
l2=0xFF^PINA;
l2=0x3C&l2;//3,4,5,6燈
l2=l2>>2;
switch(l2)
{case 0x00: discrepancy=0; break; //0000b//未偏
case 0x01: discrepancy=1; break; //0001b右偏
case 0x02: discrepancy=2; break; //0010b左偏
case 0x04: discrepancy=0; break;
case 0x05: discrepancy=1; break;
case 0x06: discrepancy=2; break;
case 0x08: discrepancy=0; break;
case 0x09: discrepancy=1; break;
case 0x0A: discrepancy=2; break;}
return(discrepancy);
}
//***************************尋線處理函數(shù)7****************************//
char OutLine(void) //線外偏離判斷
{char discrepancy;
char l2;
l2=PINA^0xFF;
l2=0x3C&l2; //00111100b
l2=l2>>2;
switch(l2)
{case 0x00: discrepancy=forflag; break; //0000b
case 0x01: discrepancy=Talone(); break; //0001b
case 0x02: discrepancy=FUalone(); break; //0010b
case 0x04: discrepancy=FIalone(); break; //0100b
case 0x05: discrepancy=0; break; //0101b
case 0x06: discrepancy=0; break; //0110b
case 0x08: discrepancy=Salone(); break; //1000b
case 0x09: discrepancy=0; break; //1001b
case 0x0A: discrepancy=0; break; //1010b
case 0x0F: discrepancy=0; break; //1111b
}
return(discrepancy);
}
//***************************校偏函數(shù)********************************//
void Revise(char discrepancy)
{if(discrepancy==0)
{forflag=0;
if(fob==1)
{PORTB=0x00;
return;}
PORTB=0x0C;
return;}
if(discrepancy==1)
{PORTB=0x08; //A為左邊電機
delay(0x0100);
PORTB=0x0C;
forflag=1;
return;}
if(discrepancy==2) //B為右邊電機
{forflag=2;
PORTB=0x04;
delay(0x0100);
PORTB=0x0C;
return;}
if(discrepancy==3)
{PORTB=0x08; //A為左邊電機
forflag=1;
return;}
if(discrepancy==4)
{forflag=2;
PORTB=0x04;
return;}
}
//***************************尋線函數(shù)********************************//
void SearchLine()
{char online; //測點狀態(tài)寄存器
char l78; //7，8號測點狀態(tài)寄存器
char y=0;
online=PINA^0xFF;
forlight=online;
l78=online&0xC0;
l78=l78>>6;
if(l78==0x03) //7，8同時亮無偏
Revise(0);
if((online&0x0D)==0x0D) //1，3，4都亮，直走
{Revise(0); //校偏函數(shù)
y=1;}
if(y==0)
{if(l78==0x01)
Revise(3); //3為大右偏
if(l78==0x02)
Revise(4); //4為大左偏
if(l78==0x00)
{online=0x03&online;
if(online==0x01) //只有1號燈亮
Revise(InLine1()); //線上走偏
if(online==0x02) //只有2號燈亮
Revise(InLine2()); //線上走偏
if (online==0x00) //1，2都不亮
Revise(OutLine());}}//線外走偏
}
//***************************停止函數(shù)********************************//
void stop()
{PORTD=PORTD|0xCF;
}
//***************************轉向函數(shù)********************************//
char action(char direction) // =1為右轉，==0為左轉，=2為停止
{char light;
char i=0;
if(direction==2)
{desireV = 0x00; //將設定速度置為0
SetOutputComReg1A(0x03FF);
SetOutputComReg1B(0x03FF);}
else
{if (direction==1)
{PORTB = 0x08; //右轉
do{
light = PINA^0xFF;
if ((light&0x44)!=0x00) //當檢測到3號燈亮轉向完成
break;}while(1);}
if (direction==0)
{PORTB = 0x04; //左轉
DELAY(6);
do{
light = PINA^0xFF;
if ((light&0x88)!=0x00) //4號燈亮轉向完成
break;}while(1);}
DELAY(2); //轉向退出延時，防止錯誤讀值
return(1);
}
}
//*******************************前進函數(shù)****************************//
void Forward(char step,char direction,char fob)
{char flagi=0; //單邊過線標志，整即過線后清0
char flagj; //過線輔助判斷標志
char middle1=0x00; //過線檢測狀態(tài)寄存器
char middle2=0x00; //前次過線檢測狀態(tài)寄存器
char postionflag=0; //位置標志
desireV = MaxSpeed;
if (fob==1) //1為后退
PORTB=0x00;
if (fob==0) //0為前進
PORTB=0x0C;
do{
flagj = 0;
middle1 = PINB^0xFF; //讀取PB口，并取反
middle1 = middle1&0xC0; //提取PB6,PB7
middle1 = middle1>>6;
if((middle1==0x03)&&(middle2!=0x03)) //兩邊同時亮且前一次不是同時亮則為過線
{postionflag++; //位置標志加1
if(postionflag==(step-1))
desireV=MidSpeed; //將設定速度降低,便于轉彎
if(postionflag==step)
{if(action(direction)==1) break;}
DELAY(30); //延時確保不重復檢測
flagi = 0;}
if(flagi!=0) //兩邊未同時過線
{if(flagi==1) //左邊先到
{if((middle1==0x02)&&((middle2&0x02)==0x00)) //檢測右邊上升沿
{postionflag++;
if(postionflag==(step-1))
desireV=MidSpeed;
if(postionflag==step)
{if(action(direction)==1) break;}
DELAY(30);
flagi = 0;
flagj = 1;}}
if(flagi==2) //右邊先到
{if((middle1==0x01)&&((middle2&0x01)==0x00)) //檢測左邊上升沿
{postionflag++;
if(postionflag==(step-1)) desireV=MidSpeed;
if(postionflag==step)
{if(action(direction)==1)
break;}
DELAY(30);
flagi = 0;
flagj = 1;}}}
if(flagj==0)
{if((middle1==0x01)&&((middle2&0x01)==0x00)) //左邊到線，右邊未到線
flagi=1;
if((middle1==0x02)&&((middle2&0x02)==0x00)) //右邊到線，左邊未到線
flagi=2;}}while(1);
return;
}
//***************************主函數(shù)**********************************//
void main()
{char movesteps; //傳遞前進或后退步數(shù)
char movedirection; //運動方向
DDRA=0x00; //PA口輸入
DDRB=0xFF; //PB口輸出
DDRC=0xFF; //PC口輸出
DDRD=0xFF; //PD口輸出
PORTA=0xFF;
PORTB=0x08; //PB2=0，PB3=1//
PORTC=0xFF;
PORTD=0x88;
SetOutputComReg1A(0x03FF);
SetOutputComReg1B(0x03FF);
Timer1Init( 0x82,0xF3,0x01);
TCNT0=0x00;
TCCR0=0x07; //T0上升沿驅(qū)動
ASSR=0x08; //外部TOSC1觸發(fā)
TCCR2=0x01;
SEI();
do{
if ((Task[time]&0x01)==1)
{if((Task[time]&0x02)==0x02) //判斷指定運動方向，1為前進
{movesteps=(Task[time]&0xF0)>>4; //取高四位，運動格數(shù)
movedirection=(Task[time]&0x0C)>>2; //提取低四位中的高兩位，轉向
Forward(movesteps,movedirection,0);}
if((Task[time]&0x02)==0x00) //判斷指定運動方向，0為后退
{movesteps=(Task[time]&0xF0)>>4; //取高四位，運動格數(shù)
movedirection=(Task[time]&0x0C)>>2; //提取低四位中的高兩位，轉向
Forward( movesteps,movedirection,1 );} }
time++;
if(time==Size) break;}while(1);
}
//***************************中斷服務程序****************************//
#pragma interrupt_handler TIM1_OVF:6
void TIM1_OVF(void) //計時器時間到,進入中斷
{char light;
CLI();
a++;
c++;
if(a==40) //速度采樣時間到
{a=0;
PIDA();
PIDB();}
if(c=100) //尋線采樣時間到
{c=0;
SearchLine();}
SEI();
}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -