用stm32f103c8t6实现平衡车的pid控制代码

实现平衡车的 PID 控制需要使用陀螺仪和加速度计来获取姿态信息，并通过 PID 控制器计算出电机的控制信号。以下是一个基于 STM32F103C8T6 的平衡车 PID 控制代码的示例：

#include "stm32f10x.h"

#define MOTOR_PIN1 GPIO_Pin_0
#define MOTOR_PIN2 GPIO_Pin_1
#define MOTOR_PIN3 GPIO_Pin_2
#define MOTOR_PIN4 GPIO_Pin_3

#define MOTOR_GPIO GPIOA

#define GYRO_OFFSET 0.0f
#define KP 10.0f
#define KI 0.5f
#define KD 2.0f

float angle = 0.0f;
float rate = 0.0f;
float error = 0.0f;
float error_sum = 0.0f;
float error_diff = 0.0f;
float last_error = 0.0f;

float motor1 = 0.0f;
float motor2 = 0.0f;
float motor3 = 0.0f;
float motor4 = 0.0f;

void init_motor(void) {
    GPIO_InitTypeDef GPIO_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GPIO_InitStructure.GPIO_Pin = MOTOR_PIN1 | MOTOR_PIN2 | MOTOR_PIN3 | MOTOR_PIN4;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(MOTOR_GPIO, &GPIO_InitStructure);
}

void set_motor(float m1, float m2, float m3, float m4) {
    uint16_t pwm1 = (uint16_t)(m1 * 1000.0f);
    uint16_t pwm2 = (uint16_t)(m2 * 1000.0f);
    uint16_t pwm3 = (uint16_t)(m3 * 1000.0f);
    uint16_t pwm4 = (uint16_t)(m4 * 1000.0f);

    TIM1->CCR1 = pwm1;
    TIM1->CCR2 = pwm2;
    TIM1->CCR3 = pwm3;
    TIM1->CCR4 = pwm4;
}

void init_timer(void) {
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_OCInitTypeDef TIM_OCInitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    TIM_TimeBaseStructure.TIM_Period = 999;
    TIM_TimeBaseStructure.TIM_Prescaler = 71;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 0;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

    TIM_OC1Init(TIM1, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);

    TIM_OC2Init(TIM1, &TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);

    TIM_OC3Init(TIM1, &TIM_OCInitStructure);
    TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);

    TIM_OC4Init(TIM1, &TIM_OCInitStructure);
    TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);

    TIM_Cmd(TIM1, ENABLE);
}

void init_gyro(void) {
    GPIO_InitTypeDef GPIO_InitStructure;
    SPI_InitTypeDef SPI_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_SPI1, ENABLE);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
    SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
    SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
    SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
    SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
    SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
    SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4;
    SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
    SPI_InitStructure.SPI_CRCPolynomial = 7;
    SPI_Init(SPI1, &SPI_InitStructure);

    SPI_Cmd(SPI1, ENABLE);
}

uint8_t spi_transfer(uint8_t data) {
    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET);
    SPI_I2S_SendData(SPI1, data);
    while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET);
    return SPI_I2S_ReceiveData(SPI1);
}

uint8_t gyro_read_byte(uint8_t address) {
    GPIO_ResetBits(GPIOA, GPIO_Pin_4);
    spi_transfer(address | 0x80);
    uint8_t value = spi_transfer(0xFF);
    GPIO_SetBits(GPIOA, GPIO_Pin_4);
    return value;
}

int16_t gyro_read_word(uint8_t address) {
    GPIO_ResetBits(GPIOA, GPIO_Pin_4);
    spi_transfer(address | 0x80);
    int16_t value = (int16_t)(spi_transfer(0xFF) << 8);
    value |= spi_transfer(0xFF);
    GPIO_SetBits(GPIOA, GPIO_Pin_4);
    return value;
}

void gyro_init(void) {
    uint8_t response = gyro_read_byte(0x75);
    if (response != 0x68) {
        while (1);
    }

    gyro_read_byte(0x6B) &= ~0x80;
    gyro_read_byte(0x6B) &= ~0x40;
    gyro_read_byte(0x6B) &= ~0x20;

    gyro_read_byte(0x1B) &= ~0x18;
    gyro_read_byte(0x1B) |= 0x08;

    gyro_read_byte(0x1C) &= ~0x18;
    gyro_read_byte(0x1C) |= 0x08;
}

void read_gyro(void) {
    int16_t raw_rate = gyro_read_word(0x43);
    rate = (float)(raw_rate - GYRO_OFFSET) / 131.0f;
}

void pid_control(void) {
    error = angle - 0.0f;
    error_sum += error;
    error_diff = error - last_error;
    last_error = error;

    float p = KP * error;
    float i = KI * error_sum;
    float d = KD * error_diff;

    float output = p + i + d;
    motor1 = 0.25f + output;
    motor2 = 0.25f + output;
    motor3 = 0.25f - output;
    motor4 = 0.25f - output;

    if (motor1 < 0.0f) motor1 = 0.0f;
    if (motor2 < 0.0f) motor2 = 0.0f;
    if (motor3 < 0.0f) motor3 = 0.0f;
    if (motor4 < 0.0f) motor4 = 0.0f;

    if (motor1 > 1.0f) motor1 = 1.0f;
    if (motor2 > 1.0f) motor2 = 1.0f;
    if (motor3 > 1.0f) motor3 = 1.0f;
    if (motor4 > 1.0f) motor4 = 1.0f;

    set_motor(motor1, motor2, motor3, motor4);
}

int main(void) {
    init_motor();
    init_timer();
    init_gyro();

    gyro_init();

    while (1) {
        read_gyro();
        angle += rate * 0.01f;
        pid_control();
        delay_ms(10);
    }
}

注意：以上示例代码仅供参考，具体实现方式可能会根据具体硬件和应用场景的不同而有所差异。