2024电赛小车记

题目如下
uploading-image-187440.png

uploading-image-613547.png

思路很简单,将读六路灰度读取

点击查看代码

void read_sensors(uint16_t *sensor_values) {

    sensor_values[0] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0);
    sensor_values[1] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1);
    sensor_values[2] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2);
    sensor_values[3] = HAL_GPIO_ReadPin(GPIOA, GPIO_P<details>
    sensor_values[4] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4);
    sensor_values[5] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_5);
}

IN_3);

将获取的电平值进行加权得到误差值以用作后面pid的参数

点击查看代码

float calculate_error(void) {
    int weights[NUM_SENSORS] = {-3, -2, -1, 1, 2, 3}; 
    float error = 0.0f;
    int active_sensors = 0; 

    for (int i = 0; i < NUM_SENSORS; i++) {
        if (sensor_values[i] == 1) {
            error += weights[i];
            active_sensors++;
        }
    }

    if (active_sensors > 0) {
        error /= active_sensors; 
    }

    return error; 
}

最后根据误差进入pid计算出pwm的comparevalue

点击查看代码

int main(void) {
    HAL_Init();                         
    stm32_clock_init(RCC_PLL_MUL9);     
  
    pwm_init(1000 - 1, 72 - 1);         

    sensor_init();                      
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
    
    PIDController pid;
   PID_Init(&pid, 96,0.01f, 1.5, 0.0f);   

    while(1) {
        
        read_sensors(sensor_values);
   
float error = calculate_error(); 
float pid_output = PID_Compute(&pid, error); 
        
        
        int16_t left_wheel_speed = 260 + (int16_t)pid_output;  
        int16_t right_wheel_speed = 260 - (int16_t)pid_output; 

        if (left_wheel_speed > 550) left_wheel_speed = 550; 
        if (left_wheel_speed < -550) left_wheel_speed = -550;      
        if (right_wheel_speed > 550) right_wheel_speed = 550; 
        if (right_wheel_speed < -550) right_wheel_speed = -550;       

        pwm_compare3_set(left_wheel_speed); 
        pwm_compare4_set(right_wheel_speed); 

        delay_ms(20); 
    }
}