机器人武术擂台---无差别组(五)推棋子算法

做一份笔记,大佬勿喷。

作者:sumjess

注意:本博客是以《2018年华北五省(市、自治区)大学生机器人大赛竞赛规则》为基础而写的

@@@@@@@@@一共写2个版本的推棋子算法-----STM32、K60@@@@@@@@

特别提醒:K60是基于山外库书写!

 

一、总述:

推棋子有一个笔者想尝试的办法,就是使用陀螺仪,这样可以让车在场地内做到定位功能,可以让车走遍及整场。但是由于时间问题,暂时还是使用最普遍的16个光电开关的方案。

无论是推棋子还是互推,前提都是不掉下擂台,所以在所有的光电开关中,起到防掉落功能的光电开关的优先级应该为最高,所以,

可能会有人想用中断来防掉落,小编认为此举是没有必要的,因为在主函数中,伪代码就是:光电开关检测-->电机执行,所以小编认为用不用中断都一样,其他的光电开关就是检测棋子的作用啦

所以整体算法部分代码就是:

 (if)         如果检测到有掉落危险,那么执行指令脱离危险。

     (else if) 否则检测哪个方向有棋子,哪侧有棋子向哪个方向开去。

      (else)   否则慢速直行

 

所以不难发现,算法都是嵌套,用的都是if,当然在这里也可以用状态机。所以小编STM32的程序用的是if嵌套,K60用的是状态机。

二、算法部分:

@@@@@@@@@STM32的推棋子算法@@@@@@@@

#define delays 380
void algor_SUM_UP(void)
{
 
	
	
      if(up_for_lift==0&&up_for_right==0&&up_back_left==0&&up_back_right==0)//没有掉落危险
      {
				/后侧检测
        if(back_1==0||back_2==0)//后侧
          {
              delay_us(200);//去抖
              if(back_1==0||back_2==0)//确定后侧
              {
                  drive_Back_H();//高速倒车
 
              }
          }
         else
              {
                  ;
              }
					/前面检测
        if(for_1==0||for_2==0||for_3==0)//前面
         {
						delay_us(200);
						if(for_1==0||for_2==0||for_3==0)//确定前面
						{
							drive_For_H();//高速直行
						}
					}
				else
					{
							;
					}
					/后侧检测--如果没有,那么直行
				if(back_1==0||back_2==0)//后侧
				{
					delay_us(200);
					if(back_1==0||back_2==0)//确定后侧
					{
						drive_Back_H();//高速后退
					}
				
				}
				else
						{
							drive_For_L();//低速直行
						}
					/前面检测--如果确实有,那么高速直行;如果没有,看最下面的else2;如果一瞬间有,看下看面的else1;
				if(for_1==0||for_2==0||for_3==0)//前面
				{
					delay_us(200);
					if(for_1==0||for_2==0||for_3==0)//确定前面
					{
						drive_For_H();//高速直行
					}
					else//else1,检测左边,如果有左转;如果一瞬间有,看下面的else3;
					{
              if(left_1==0||left_2==0||for_lift==0||back_lift==0)//左侧
              {
                delay_us(200);
                if(left_1==0||left_2==0||for_lift==0||back_lift==0)//确定左侧
                  {
                    drive_Left_S();//超级左转
                    delay_ms(delays);
										
										
                   }
							}
              else//else3,如果有右转;如果一瞬间有低速直行;
                  {
                        if(right_1==0||right_2==0||for_right==0||back_right==0)//右侧
                        {
                            delay_us(200);
                            if(right_1==0||right_2==0||for_right==0||back_right==0)//确定右侧
                              {
                                    drive_Right_S();//超级右转
                                    delay_ms(delays);
                              }
                             else
                             {
                               drive_For_L();//低速直行
                             }
                         }
                  }
              
       }
 
 
  }
				
  else//检测左侧,如果有左转;如果一瞬间有,检测右侧;如果右侧确实有,右转;如果右侧一瞬间有,那么低速直行;
  {
             if(left_1==0||left_2==0||for_lift==0||back_lift==0)//左侧
              {
                delay_us(200);
                if(left_1==0||left_2==0||for_lift==0||back_lift==0)//确定左侧
                  {
                        drive_Left_S();//左转
                        delay_ms(delays);
                   }
							}
              else
                  {
                        if(right_1==0||right_2==0||for_right==0||back_right==0)//右侧
                        {
                            delay_us(200);
                            if(right_1==0||right_2==0||for_right==0||back_right==0)//确定右侧
                                  {
                                    drive_Right_S();//右转
                                    delay_ms(delays);
                                   }
                             else
                             {
                               drive_For_L();//低速直行
                             }
                         }
                  }
             
  }
}
			      else
      {
        if(up_for_lift==1||up_for_right==1)//前方有掉落危险
        {
          delay_us(200);
          if(up_for_lift==1||up_for_right==1)//确定有掉落危险
          {
 
            drive_Stop();//停下驱动
            drive_Back_H();//高速倒车
            delay_ms(700);//300ms
            drive_Right();//右转(可以设置向左向右算法)
            delay_ms(500);//250ms
          }
        }
        if(up_back_left==1||up_back_right==1)//后方有掉落危险
        {
          delay_us(200);
          if(up_back_left==1||up_back_right==1)//确定有掉落危险
          {
						drive_Stop();//停下驱动J
            drive_For_M();//快速直行
            delay_ms(700);//300msJ
            drive_Right();//右转(可以设置向左向右算法)J
            delay_ms(500);//250msJ
          }
        }
			}
		}
 

@@@@@@@@@K60的推棋子算法@@@@@@@@@@

1、扫描函数:

    uint8 opt_qua_Scan(void)//排位模式扫描程序
  {
    int sensorState;
          if( left_forward_edge  == 1 )    { sensorState =   1; }//左前边缘
    else if ( right_forward_edge  == 1 )   { sensorState =  2;  }//右前边缘
    else if ( left_back_edge  == 1 )       { sensorState =  3;  }//左后边缘
    else if ( right_back_edge  == 1 )      { sensorState =  4;  }//右后边缘
    else if ( left_forward  == 0 )         { sensorState =  5;  }//左后
    else if ( forward_middle_left  == 0 )  { sensorState =  6;  }//前中左
    else if ( forward_middle_right == 0 )  { sensorState =  7;  }//前中右
    else if ( right_forward == 0 )         { sensorState =  8;  }//右前
    else if ( left_1  == 0 )               { sensorState =  9;  }//左一
    else if ( right_1  == 0 )              { sensorState =  10; }//右一
    else if ( left_2  == 0 )               { sensorState =  11; }//左二
    else if ( right_2 == 0 )               { sensorState =  12; }//右二
    else if ( left_back == 0 )             { sensorState =  13; }//左后
    else if ( back_middle_left == 0 )      { sensorState =  14; }//后中左
    else if ( back_middle_right == 0 )     { sensorState =  15; }//后中右
    else if ( right_back == 0 )            { sensorState =  16; }//右后
    else    {                                 sensorState = 17; }//巡逻模式
    return sensorState;
  }
 

2、执行函数:

t=opt_qua_Scan();	//得到键值
			switch(t)
		        {				 
			case 1:
			{	
              //右后          
			  sumjess_back_right_fastest();   
                         break;       
                        }
			case 2:
			{	
              //左后            
			  sumjess_back_left_fastest(); 
                         break;       
                        }
			case 3:				
			{	
              //右前            
			  sumjess_forward_right_fastest(); 
                         break;       
                        }
			case 4:
			{	
              //右前            
			  sumjess_forward_right_fastest();
                         break;       
                        }
			case 5:
			{	
              //左后            
			  sumjess_back_left_faster(); 
                         break;       
                        }
			case 6:				
			{	
              //前左            
			  sumjess_forward_left_fast();
                         break;       
                        }
			case 7:
			{	
              //前右           
			  sumjess_forward_right_faster();
                         break;       
                        }
			case 8:
			{	
              //前右            
			  sumjess_forward_right_fastest(); 
                         break;       
                        }
			case 9:				
			{	
              //前左            
			  sumjess_back_left_fastest(); 
                         break;       
                        }
			case 10:
			{	
               //右后            
			   sumjess_back_right_fastest();
                         break;       
                        }
			case 11:
			{	
              //前左            
			  sumjess_back_left_fastest();
                         break;       
                        }
			case 12:				
			{	
              //前右            
			  sumjess_forward_right_fastest();
                         break;       
                        }
			case 13:
			{	
              //左后            
			  sumjess_back_left_faster(); 
                         break;       
                        }
			case 14:
			{	
              //左后            
			  sumjess_back_left_faster(); 
                         break;       
                        }
			case 15:				
			{	
               //右后            
			   sumjess_back_right_fastest();
                         break;       
                        }
			case 16:
			{	
               //右后          
			   sumjess_back_right_faster();
                         break;       
                        }
			case 17:
			{	
             //中速前进             
			 sumjess_forward_faster();  
                         break;       
                        }
                              
			default:
			         break;//跳出
			}

 

 

注:以上所有配置不一定通用,请各位读者按照自己板子的型号和原理图进行调整。