robbot._c

寻迹机器人设计开发环境为ICC,可以编译成汇编,直接下栽到嵌入式avr单片机进行控制

//***************************FileName:Robbot.c***********************//
//***************************ICCAVR6.30编译**************************//

#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);
 }
} 
   								 
//*******************************前进函数****************************//
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();
}