zhudang0617.c

自主移动机器人控制程序

/*------------------------------------------------
//阻挡机器人程序
//时间：2004年6月11日
//C口2/3/4/5为两个主电机的控制
//PORTD 6/7是横走电机控制，通过两个继电器实现PD6低电平是在左边场地，
  PD7低电平在右边场地
//PORTA 光电寻线，加上横线数线
//PORTD 2/3 分别是数线和停止检测

------------------------------------------------*/
/**************宏定义区****************************/
#include <iom16v.h>
#include <macros.h>
#include <eeprom.h>
/**************全局变量***************************/
//按键显示
const char DISCODE[] ={0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,
                       0x7c,0x39,0x5e,0x79,0x71,0x00,0x40,0x63,0x70,0x46};
unsigned char  Disp_Buff[]={0,0,0,0,};					  
unsigned char  disp_key_count =0;                       //LED扫描计数
char  key_num =0x0f;
char  old_key_num =0;
char  new_key_num =0;
char  key_buff[] ={0,0,0,0};      
int   key_down_time =0;
char  key_delay =0;
int   time_count_sys =0;                        //系统时间计数，1ms为单位
//delay time
unsigned int delay_time_count =0;

//motor
unsigned char pwm_count =0; 
unsigned char speed =0;
unsigned char  L_speed =0;
unsigned char  R_speed =0;
const unsigned char  SPEED_H[] ={0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
const unsigned char  SPEED_M[] ={0,0,1,2,3,4,5,6,7,8, 9,10,11,12,13,14};
const unsigned char  SPEED_L[] ={0,0,0,0,0,0,0,1,2,3, 4, 5, 5, 6, 7, 8};
//follow_line
char follow_line_en =0;
//ADC
unsigned int temp =0;
unsigned char adc_channel =0;
unsigned int adc_value1_buff[] ={0,0,0,0,0,0,0,0,};
unsigned int adc_value2_buff[] ={0,0,0,0,0,0,0,0,};
unsigned int adc_value[] ={0,0,0,0,0,0,0,0,};
unsigned int adc_value_buff[] ={0,0,0,0,0,0,0,0,};

unsigned char adc_start_flag =0;
unsigned char adc_get_count =0;
unsigned char adc_value_flag =0;
//H line count
unsigned char line_H_cont =0;
unsigned char line_cont_juge =0;
unsigned char line_cont_en =0;
unsigned char line_H_en =0;
//V Line count
unsigned char V_Line_juge =0;
unsigned char Line_V =0;
unsigned char V_cont_en =0;//开启横线数线
unsigned char V_Line_en =0;
//stop juge
unsigned char stop_cont_juge =0;
unsigned char stop_cont =0;
unsigned char stop_juge_en =0;
unsigned char stop_en =0;
//过程变量
unsigned char prog_step =0;
unsigned char robot_go =0;
unsigned char DirectSelect =0;
unsigned char ModelSelect =0;
//中断
#pragma  interrupt_handler timer0_ovf:10
//#pragma  interrupt_handler int0_ovf:2
/**************函数申明***************************/
/*  初始化  */
void init_devices(void);
void port_init(void);
//void init_ext(void);
void timer_init(void);
void adc_init(void);
//display & key
void send_data_disp(unsigned char send_data) ;
/*  过程控制函数  */
void followline(unsigned char go_speed,unsigned char direct);
void robot_stop(void);
void Line_Count(void);
void Stop_juge(void);
void Hand_A_open(void);
void Line_V_cont(void);
/*  比赛策略函数     */
void MODEL_A(void);
void MODEL_B(void);
void MODEL_B(void);
void MODEL_B(void);
/**************子函数区***************************/
//----------------------------------------
/*  初始化函数  */
void  port_init(void)
{
      DDRA  = 0x00;           
 	  PORTA = 0x00;           //光电管AD
 	  DDRB  = 0B11100000;
 	  PORTB = 0B00011111;
 	  DDRC  = 0B11111100;				   					 
 	  PORTC = 0B11111100;          //               
 	  DDRD  = 0B11110000;	        //PD5
 	  PORTD = 0B11111111;
}
void timer_init(void)
{	 	
      TCCR0 =3;		 //32分频
	  TCNT0 =0x83;
      TIMSK =0x41;			   //open timer0  and timer2	中断使能			   
}
void adc_init(void)
{
      ADMUX = 0;                  //ADC第0通道输入
 	  ADCSRA = 0B10010001;//2分频 //0b10010011;        //开ADC，并8分频
}
void init_ext(void)
{
 	 GICR=0x00;	   			   	//INT0、INT1、INT2请求外中断触发 									
 	 MCUCR = 0b00001010;        //开中断0、1，下降沿触发脉冲
// 	 MCUCSR &= 0b10111111;		//外中断2下降沿触发
 	 GIFR=0Xff;	  	   			//中断标志寄存器置1清零
 	 //GICR=0B11000000;          //开外中断
}
void init_devices(void)

{
      CLI(); 		   	   		 //关中断  
 	  port_init();				 //端口初始化
     // init_ext();				 //外中断初始化
 	  adc_init();                 //adc init
	  timer_init();				 //定时器初始化
	  SEI();						 //开中断
}
void disp_bit(char disCodeIndex,char bitChoose)     
{//按位显示
      PORTB &= 0B11011111;//下拉RCK
   	  send_data_disp(bitChoose);//送位选
   	  send_data_disp(DISCODE[disCodeIndex]);//送段码
   	  PORTB |= 0B00100000;//反向RCK
}
void send_data_disp(unsigned char send_data)       
{//向595送数据串
      unsigned char count;
   	  for(count =0;count<8;count++)
   	  {
            if(((send_data<<count)&0x80 )==0)
	     	      PORTB &=0B01111111;
      		else
	     	      PORTB |=0B10000000;
	  		PORTB |=0B01000000;//送时钟SCK
      		PORTB &=0B10111111;
   	  }
}
void Disp_key_group(void)    
{ //显示和键值获取，使用时只需对Disp_Buff[] 刷新。
      disp_key_count++;
	  if(disp_key_count>3)    //4个LED的板子(disp_key_count>3)
	        disp_key_count =0;
	  disp_bit(Disp_Buff[disp_key_count],1 <<disp_key_count);
      if((PINB & 1) ==0)//有键按下
	        key_buff[disp_key_count] =1;
      else 
		    key_buff[disp_key_count] =0;
}
void Key_num(void)
{// 键值处理及去抖 
      if(key_buff[disp_key_count] ==1)
	        new_key_num =disp_key_count;
      else 
            new_key_num =0x0f;
  	  if((old_key_num ==0x0f) && (new_key_num !=0x0f) && (key_delay ==0))//判断按键正按下
            key_num =new_key_num;
      else if ((old_key_num !=0x0f) && (new_key_num ==old_key_num))//判断按键持续按下
      {
            key_num =0x0f ;//持续按键时不再给出键值,只给出空键值。   
	        key_down_time++;//记录按键持续按下时间
      }//稍微修改一下可以得到比较完整的键盘返回码
           else 
  		   {   
	 	         if((old_key_num !=0x0f) && (new_key_num ==0x0f))//按键释放
     	       	 {  
           	   	       key_num =0x0f;
	       	   	       key_down_time =0;//按键时间变量清零
		   	   	 	   key_delay=50;//去抖动延时50ms
     	   	   	 }
  		   }
      old_key_num =new_key_num;
}
/* ADC */
Get_ADC_value(char channel)
{    //ADC读入
      ADMUX =channel;
   	  ADCSRA |=0B01000000;
   	  while((ADCSRA &0B00010000) ==0);
   	  temp =ADC;
   	  ADCSRA |=0B00010000;
   	  return temp;
}
/*  ADC value Flag get ：和与之阈值比较得到判断标志    */
void ADC_valueFlag_get(void)
{
      char i;
	  for(i =0;i <8;i++)
  	  { 
  	        char tempa;
  	  		if(adc_value_buff[i] >=adc_value[i])
	  		{
	  		      tempa =(1<<i);
	  			  adc_value_flag |=tempa;
	  		}
  	  		else
	 		{
	 		      tempa =~(1<<i);
	 		      adc_value_flag &=tempa;
	 		}
      }
}  
/*  ms精确延时函数  */
void delay(unsigned int n)
{//延时
      delay_time_count =n;
	  while(delay_time_count >0);
}
//----------------------------------------------
/*   机器人行走  */
void ADC_base_get(void)
{//阈值采集
      char i;
	  Disp_Buff[2] =17;
	  Disp_Buff[3] =17;
	  for(i =0;i <8;i++)
	        Get_ADC_value(i);//处次采样，防止ADC误差
	  for(i =0;i <8;i++)
	  {
	       adc_value1_buff[i] =Get_ADC_value(i);//第一次采样
	  }
	  while(prog_step ==1)
	  {
		   Disp_Buff[0] =adc_value1_buff[0]/64;
	       Disp_Buff[1] =1;
	  }
	  for(i =0;i <8;i++)
	  {
	        adc_value2_buff[i] =Get_ADC_value(i);//第二次采样
   	  }
	  Disp_Buff[0]=adc_value2_buff[0]/64;//显示第二次采样值
	  Disp_Buff[1] =2;
	  for(i =0;i <8;i++)
	  {
	        adc_value[i] =(adc_value1_buff[i]+adc_value2_buff[i])/2;//计算中值
      }
	  EEPROMWriteBytes(0x0000,adc_value,16);     //阈值存入EEPROM中
	  delay(600);
	  prog_step =0;//跳回初始状态
}
//follow line

void followline(unsigned char go_speed,unsigned char direct)
{   //寻线主函数
      if(direct ==1)
  	  {
            switch(adc_value_flag)
      		{
                  case 0b00011000://全部在白线上机器人直走
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_H[go_speed];
	     		  	   break;
	        	  case 0b00011100://稍微在白线的左边，机器人向右微调
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_M[go_speed];  
	        	 	   break;
				  case 0b00001100://稍微在白线的左边，机器人向右微调
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_M[go_speed];  
	        	 	   break;
				  case 0b00001110://稍微在白线的左边，机器人向右调整
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_L[go_speed];  
	        	 	   break;
				  case 0b00000110://稍微在白线的左边，机器人向右调整
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_L[go_speed];  
	        	 	   break;
				  case 0b00000111://稍微在白线的左边，机器人向右调整
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_L[go_speed];  
	        	 	   break;
				  case 0b00000011://稍微在白线的左边，机器人向右调整
	     		  	   L_speed =SPEED_H[go_speed];
	     		 	   R_speed =SPEED_L[go_speed];  
	        	 	   break;	   	   
				  case 0b00111000://稍微在白线的右边，机器人向左微调
	     		  	   L_speed =SPEED_M[go_speed];
	     		 	   R_speed =SPEED_H[go_speed]; 
	     		 	   break; 
				  case 0b00110000://稍微在白线的右边，机器人向左微调
	     		  	   L_speed =SPEED_M[go_speed];
	     		 	   R_speed =SPEED_H[go_speed]; 
	     		 	   break; 	        
                  case 0b01110000://在白线的右边，机器人向左调整
	     		  	   L_speed =SPEED_L[go_speed];
	     		 	   R_speed =SPEED_H[go_speed];
	     		 	   break;
				  case 0b01100000://在白线的右边，机器人向左调整
	     		  	   L_speed =SPEED_L[go_speed];
	     		 	   R_speed =SPEED_H[go_speed];
	     		 	   break;
			      case 0b11100000://在白线的右边，机器人向左调整
	     		  	   L_speed =SPEED_L[go_speed];
	     		 	   R_speed =SPEED_H[go_speed];
	     		 	   break;
				  case 0b11000000://在白线的右边，机器人向左调整
	     		  	   L_speed =SPEED_L[go_speed];
	     		 	   R_speed =SPEED_H[go_speed];
	     		 	   break;	   		   	   
	       		 default:
	   	 		 	   break;      
      		}
    }
}

void motor_PWM(void)
{
    if(pwm_count <L_speed)
	{
	   PORTC &=0b11111011;
	}
	else
	{
	   PORTC |=0b00000100;
	}
    if(pwm_count <R_speed)
	{
	   PORTC &=0b11101111;
	}
	else
	{
	   PORTC |=0b00010000;
	}
}
void ROBOT_GO(unsigned L_SPEED,unsigned R_SPEED)
{
    L_speed =L_SPEED;