使用灰度传感器循迹

uint8_t stop_way = 0;//停止线检测

uint8_t sensor_val[5];//灰度返回值

uint16_t gray_state = 0x0;//当前灰度状态

uint16_t gray_status[2]={0},gray_status_backup[2][20]={0};//灰度传感器状态与历史值

uint32_t gray_status_worse=0;        //灰度管异常状态计数器

 

/****************************

函数名称：

函数作用：5路循迹获取状态

函数参数：无

函数返回值：当前巡线状态 16进制

****************************/

uint16_t Sensor_GetState(void)

{

        uint16_t State = 0X0000;

        

//        sensor_val[0] = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_0);

//        sensor_val[1] = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_1);

//        sensor_val[2] = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_2);

//        sensor_val[3] = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_3);

//        sensor_val[4] = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_4);

//        

//        State |= (sensor_val[4] << 4);

//        State |= (sensor_val[3] << 3);

//        State |= (sensor_val[2] << 2);

//        State |= (sensor_val[1] << 1);

//        State |= (sensor_val[0] << 0);

        

        for(int8_t i=4;i>=0;i--)

        {

                sensor_val[i] = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_0<<i);

                State |= (sensor_val[i]<<i);

        }

        

        return State;

}

 

void gray_check(void)

{

        gray_state = Sensor_GetState();

        

        //记录上一次传感器量

        for(uint16_t i=19;i>0;i--)

        {

            gray_status_backup[0][i]=gray_status_backup[0][i-1];

        }

        

        gray_status_backup[0][0]=gray_status[0];

        

        //灰度检测

        switch(gray_state)

        {

                case 0x01:gray_status[0] = 4; gray_status_worse/=2;break;          //00001b

                case 0x03:gray_status[0] = 3; gray_status_worse/=2;break;          //00011b

            case 0x02:gray_status[0] = 2;        gray_status_worse/=2;break;   //00010b

                case 0x06:gray_status[0] = 1;        gray_status_worse/=2;break;          //00110b

                case 0x04:gray_status[0] = 0;        gray_status_worse/=2;break;   //00100b

                case 0x0C:gray_status[0] = -1;gray_status_worse/=2;break;                //01100b

                case 0x08:gray_status[0] = -2;gray_status_worse/=2;break;                //01000b

                case 0x18:gray_status[0] = -3;gray_status_worse/=2;break;                //11000b

                case 0x10:gray_status[0] = -4;gray_status_worse/=2;break;                //10000b

                case 0x00:gray_status[0] = gray_status_backup[0][0];gray_status_worse++;break; //00000b

                

                default://其它特殊情况单独判断

                {

                        gray_status[0]=gray_status_backup[0][0];

                        gray_status_worse++;

                }

        }        

        

                switch(gray_state)//停止线检测

                {

                        case 0x0F://01111b

                        case 0x1E://11110b

                        case 0x1F://11111b

                        {

                                stop_way++;

                        }

                        break;

                }                

}

/**

  * [url=home.php?mod=space&uid=247401]@brief[/url]  The application entry point.

  * @retval int

  */

int main(void)

{

  /* USER CODE BEGIN 1 */

 

  /* USER CODE END 1 */

 

  /* MCU Configuration--------------------------------------------------------*/

 

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */

  HAL_Init();

 

  /* USER CODE BEGIN Init */

 

  /* USER CODE END Init */

 

  /* Configure the system clock */

  SystemClock_Config();

 

  /* USER CODE BEGIN SysInit */

 

  /* USER CODE END SysInit */

 

  /* Initialize all configured peripherals */

  MX_GPIO_Init();

  MX_TIM3_Init();

  MX_TIM2_Init();

  MX_TIM4_Init();

  /* USER CODE BEGIN 2 */

                HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);

                HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);

                

                HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);

                HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);

 

  /* USER CODE END 2 */

 

  /* Infinite loop */

  /* USER CODE BEGIN WHILE */

  while (1)

  {

                motor_ctrl(500+gray_status[0]*100,-(500-gray_status[0]*100));

                Get_Encoder();

    /* USER CODE END WHILE */

 

    /* USER CODE BEGIN 3 */

  }

  /* USER CODE END 3 */

}