电机控制

void Motor2_H_init(void)
{
	U8 step_run = 0;
	U8 tmp1,tmp2;
	U32 runtime1=0,timerun2=0;
	U8 flag=0;
    for(;;)
    {
		tmp1=GPIO_ReadInputDataBit(GPIOB,GPIO_PIN_3);
		tmp2=GPIO_ReadInputDataBit(GPIOC,GPIO_PIN_11);
        switch(step_run)
        {
           case 0:
		   	{
               if((tmp1==1)&&(tmp2==1))
               	{
				   step_run=2;
			    }else
				{
                   step_run=1; 
				   timerun2=0;
				}
		    }
		   	break;
			case 1:
			{
				timerun2++;
				if(timerun2<2500)
				{
					M24_L2 = HIGH;//逆时针转
					HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_0);
				}else if(timerun2<5000)//到这个时候还没检查到两光眼信号说明要卡住了
				{
					M24_R2 = HIGH;//顺时针转
					HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_1);
				}else if(timerun2>5001)
				{
                     //超时光眼坏了
                     step_run=5;
				}
				if((tmp1==1)&&(tmp2==1))
				{
					step_run=0;
				}		
			}
			break;
			case 2:
				{
					M24_R2 = HIGH;//顺时针转
					HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_1);
					if(tmp1==0) //在不正常位置
					{
						step_run=3;
					}
					if(tmp2==0)//在正常位置
					{
						step_run=4;
					}

			    }
			break;
			case 3:
				{
					M24_R2 = HIGH;//顺时针转
					HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_1);
					if((tmp1==1)&&(tmp2==1))
					{
						M24_R2 = HIGH;
						M24_L2 = HIGH;
						step_run=5;
					}

			    }
			break;
			case 4:
				{
					M24_L2 = HIGH;//逆时针转
					HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_0);
					if((tmp1==1)&&(tmp2==1))
						{
							M24_R2 = HIGH;
							M24_L2 = HIGH;
							step_run=5;
						}
			    }
			break;
			case 5:
				{
					flag=1;
			    }
				break;
		}

	   if(++runtime1 >= 2500)//超时处理
	   {
	      flag=1;
	   }
	   os_dly_wait(3);
       if(flag>1) break;
	}
    	
}