尋線機器人系統設計程序

#include <io8535v.h>
#include <macros.h>

//****************************宏定義********************************//
#define Aa 0.5               //PID參數
#define Ba -0.5              //PID參數
#define Ca 0                 //PID參數
#define Ab 0.09              //PID參數
#define Bb -0.1              //PID參數
#define Cb 0                 //PID參數
#define U1 12                //PID參數
#define MaxSpeed 0x40        //最大速度
#define MidSpeed 0x18        //中速,用於轉彎
#define Size 6               //任務數組大小

//*****************************全局變量定義**************************//
char sflag=0x00;            //記錄上一次校偏狀態
char crossflag = 0;         //過線標誌，用於判斷是否過線
char forflag=0;              //記錄上一次機器人行進狀態
char forlight;               //記錄上一次A口光電傳感器的狀態
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;                   //溢出次數，控制PID窗口時間
char c=0;                   //控制尋線頻率
int   desireV=10;
char b=0;
char time=0;                 //機器人行走步數
char fob=0;                 //=1後退，=0前進
char Task[Size]={0x37,0x27,0x1B,0x29,0x23,0x1B};//任務數據數組

//******************************定時器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);
}

//******************************長延時函數***************************//
void DELAY(int delaytime)
{int i,j;
for(i=0;i<=delaytime;i++)
{for (j=0;j<=0xFFFE;j++) ;}
}

//******************************短延時函數***************************//
void delay(int i)
{int j;
for(j=0;j<=i;j++) ;
}

//******************************PID調節:左電機***********************//
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調節:右電機***********************//
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;
}

//***************************尋線處理函數1***************************//
char Talone()
{if ((forlight&0x09)==0)     //3單獨亮
    return(0);
else
    return(1);               //返回爲校偏狀態值
}

//***************************尋線處理函數2***************************//
char FUalone()               //4單獨亮
{if ((forlight&0x05)==0)
    return(0);
else
    return(2);
}

//***************************尋線處理函數3***************************//
char FIalone()               //5單獨亮
{if ((forlight&0x22)==0)
    return(0);
else
    return(1);
}

//***************************尋線處理函數4***************************//
char Salone()                //6單獨亮
{if ((forlight&0x12)==0)
    return(0);
else
    return(2);
}

//***************************尋線處理函數5****************************//
char InLine1(void)           //線上偏離判斷
{char discrepancy;           //行進狀態
char l2;                    //傳感器狀態
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);
}

//***************************尋線處理函數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);
}

//***************************尋線處理函數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);
}

//***************************校偏函數********************************//
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;}
}

//***************************尋線函數********************************//
void SearchLine()
{char online;               //測點狀態寄存器
char l78;                  //7，8號測點狀態寄存器
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);                //校偏函數
   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());}}//線外走偏
}

//***************************停止函數********************************//
void stop()
{PORTD=PORTD|0xCF;
}

//***************************轉向函數********************************//
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);}
return(0);
}

//*******************************前進函數****************************//
void Forward(char step,char direction,char fob)
{char flagi=0;               //單邊過線標誌，整即過線後清0
char flagj;                 //過線輔助判斷標誌
char middle1=0x00;          //過線檢測狀態寄存器
char middle2=0x00;          //前次過線檢測狀態寄存器
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;
}

//***************************主函數**********************************//
void main()
{char movesteps;            //傳遞前進或後退步數
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上升沿驅動
ASSR=0x08;                 //外部TOSC1觸發
TCCR2=0x01;
SEI();
do{
     if ((Task[time]&0x01)==1)
      {if((Task[time]&0x02)==0x02)              //判斷指定運動方向，1爲前進
     {movesteps=(Task[time]&0xF0)>>4;        //取高四位，運動格數
      movedirection=(Task[time]&0x0C)>>2;    //提取低四位中的高兩位，轉向
      Forward(movesteps,movedirection,0);}
       if((Task[time]&0x02)==0x00)              //判斷指定運動方向，0爲後退
     {movesteps=(Task[time]&0xF0)>>4;        //取高四位，運動格數
      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();
}

尋線機器人系統設計程序

Python 潮流週刊#52：Python 處理 Excel 的資源

別墅區可視對講系統

PWM使用

直流電機驅動器設計的應用程序

同步串口spi的c語言編程

尋線機器人系統設計程序

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結