PID 巡线

#include "Tracking.h"

uint8_t Sensor_Value[8],Stop_Counter;
uint16_t Gray_State,Gray_LastState;
int16_t Gray_Weight,Gray_LastWeight;
bool Scan_State = false;

//灯亮浅色255低电平
//灯灭深色0高电平
uint16_t Tracks_Sensor_GetValue(void)
{
    uint16_t State = 0x00;
    
    Sensor_Value[0] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_8);
    Sensor_Value[1] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_7);
    Sensor_Value[2] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_6);
    Sensor_Value[3] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_5);
    Sensor_Value[4] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_4);
  Sensor_Value[5] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_3);
    Sensor_Value[6] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_2);
    Sensor_Value[7] = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_1);
    
    State |= (Sensor_Value[0]<<7) | (Sensor_Value[1] << 6) | (Sensor_Value[2] << 5) \
          |  (Sensor_Value[3] << 4) | (Sensor_Value[4] << 3) | (Sensor_Value[5] << 2) \
          |  (Sensor_Value[6] << 1) | (Sensor_Value[7] << 0);
    return State;
}
int Huidu_Sum;

float Tracks_GrayJudgement(void)
{
    Gray_State = Tracks_Sensor_GetValue();
    
    Huidu_Sum = 0;
    for(int i=0;i<7;i++)
    {
        if((Gray_State>>i)&0x01) Huidu_Sum++;
    }
    if(Huidu_Sum<=2)//在巡线
    {
       switch(Gray_State)
      {
          case 0x40:Gray_Weight = 8;break;//0100  0000
          case 0x60:Gray_Weight = 6;break;//0110  0000
          case 0x20:Gray_Weight = 4;break;//0010  0000
          case 0x30:Gray_Weight = 2;break;//0011  0000
          case 0x18:Gray_Weight = 0;break;//0001  1000
          case 0x10:Gray_Weight = 0;break;//0001  0000
          case 0x08:Gray_Weight = 0;break;//0000  1000
          case 0x0c:Gray_Weight = -2;break;//0000  1100
          case 0x04:Gray_Weight = -4;break;//0000  0100
      case 0x06:Gray_Weight = -6;break;//0000  0110
          case 0x02:Gray_Weight = -8;break;//0000  0010
          default:
              Gray_Weight =Gray_LastState;
          break;
      }
    }
    
    
    

    Gray_LastWeight=Gray_Weight;
    return Gray_Weight;
}

uint8_t Tracks_GetStopFlag(void)
{
    return Stop_Counter;
}