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

平衡车的PID控制是一个比较复杂的算法，需要综合考虑多个因素来控制车身的平衡，比如车身的倾斜角度、加速度、角速度等等。下面是一个基于STM32F103C8T6的简单平衡车PID控制代码，供您参考。

#include "stm32f10x.h"

#define MOTOR1_PIN1 GPIO_Pin_0
#define MOTOR1_PIN2 GPIO_Pin_1
#define MOTOR2_PIN1 GPIO_Pin_2
#define MOTOR2_PIN2 GPIO_Pin_3
#define MOTOR_PORT GPIOA

#define MPU6050_ADDRESS 0xD0
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_ZOUT_H 0x3F

#define Kp 4.0f
#define Ki 0.2f
#define Kd 0.0f

float angle, gyro, balance;
float last_error, integral, derivative;
float pid_output;

void init_motor(void);
void init_mpu6050(void);
float get_angle(void);
float get_gyro(void);
float get_balance(void);
void set_motor(float speed1, float speed2);
void pid_control(void);

int main(void)
{
    init_motor();
    init_mpu6050();

    while (1)
    {
        pid_control();
    }
}

void init_motor(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GPIO_InitStructure.GPIO_Pin = MOTOR1_PIN1 | MOTOR1_PIN2 | MOTOR2_PIN1 | MOTOR2_PIN2;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(MOTOR_PORT, &GPIO_InitStructure);
}

void init_mpu6050(void)
{
    uint8_t data;

    I2C1->CR1 |= I2C_CR1_PE;
    RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;

    GPIOB->CRL |= GPIO_CRL_MODE6 | GPIO_CRL_MODE7;
    GPIOB->CRL |= GPIO_CRL_CNF6_1 | GPIO_CRL_CNF7_1;

    I2C1->CR2 |= 0x08;
    I2C1->CCR |= 0x50;
    I2C1->TRISE |= 0x09;

    I2C1->CR1 |= I2C_CR1_ACK;
    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (!(I2C1->SR2 & I2C_SR2_TRA));
    I2C1->DR = MPU6050_PWR_MGMT_1;
    while (!(I2C1->SR1 & I2C_SR1_TXE));
    I2C1->CR1 |= I2C_CR1_STOP;

    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (!(I2C1->SR2 & I2C_SR2_TRA));
    I2C1->DR = MPU6050_GYRO_XOUT_H;
    while (!(I2C1->SR1 & I2C_SR1_TXE));
    I2C1->CR1 |= I2C_CR1_STOP;

    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS + 1;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (I2C1->SR2 & I2C_SR2_TRA);
    I2C1->CR1 |= I2C_CR1_ACK;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    I2C1->CR1 |= I2C_CR1_STOP;
}

float get_angle(void)
{
    uint8_t data;
    int16_t accel_x, accel_y, accel_z;

    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (!(I2C1->SR2 & I2C_SR2_TRA));
    I2C1->DR = MPU6050_ACCEL_XOUT_H;
    while (!(I2C1->SR1 & I2C_SR1_TXE));
    I2C1->CR1 |= I2C_CR1_STOP;

    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS + 1;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (I2C1->SR2 & I2C_SR2_TRA);
    I2C1->CR1 |= I2C_CR1_ACK;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    accel_x = (data << 8) | I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    accel_y = (data << 8) | I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    accel_z = (data << 8) | I2C1->DR;
    I2C1->CR1 |= I2C_CR1_STOP;

    angle = atan2(accel_y, accel_z) * 180 / 3.14;

    return angle;
}

float get_gyro(void)
{
    uint8_t data;
    int16_t gyro_x, gyro_y, gyro_z;

    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (!(I2C1->SR2 & I2C_SR2_TRA));
    I2C1->DR = MPU6050_GYRO_XOUT_H;
    while (!(I2C1->SR1 & I2C_SR1_TXE));
    I2C1->CR1 |= I2C_CR1_STOP;

    I2C1->CR1 |= I2C_CR1_START;
    while (!(I2C1->SR1 & I2C_SR1_SB));
    I2C1->DR = MPU6050_ADDRESS + 1;
    while (!(I2C1->SR1 & I2C_SR1_ADDR));
    while (I2C1->SR2 & I2C_SR2_TRA);
    I2C1->CR1 |= I2C_CR1_ACK;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    gyro_x = (data << 8) | I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    gyro_y = (data << 8) | I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    data = I2C1->DR;
    while (!(I2C1->SR1 & I2C_SR1_RXNE));
    gyro_z = (data << 8) | I2C1->DR;
    I2C1->CR1 |= I2C_CR1_STOP;

    gyro = gyro_y / 131.0f;

    return gyro;
}

float get_balance(void)
{
    angle = get_angle();
    gyro = get_gyro();
    balance = Kp * angle + Ki * integral + Kd * derivative;
    last_error = angle;
    integral += angle;
    derivative = angle - last_error;

    return balance;
}

void set_motor(float speed1, float speed2)
{
    if (speed1 > 0)
    {
        GPIO_SetBits(MOTOR_PORT, MOTOR1_PIN1);
        GPIO_ResetBits(MOTOR_PORT, MOTOR1_PIN2);
        TIM4->CCR1 = (uint16_t)(speed1 * 1000);
    }
    else if (speed1 < 0)
    {
        GPIO_ResetBits(MOTOR_PORT, MOTOR1_PIN1);
        GPIO_SetBits(MOTOR_PORT, MOTOR1_PIN2);
        TIM4->CCR1 = (uint16_t)(-speed1 * 1000);
    }
    else
    {
        GPIO_ResetBits(MOTOR_PORT, MOTOR1_PIN1);
        GPIO_ResetBits(MOTOR_PORT, MOTOR1_PIN2);
        TIM4->CCR1 = 0;
    }

    if (speed2 > 0)
    {
        GPIO_SetBits(MOTOR_PORT, MOTOR2_PIN1);
        GPIO_ResetBits(MOTOR_PORT, MOTOR2_PIN2);
        TIM4->CCR2 = (uint16_t)(speed2 * 1000);
    }
    else if (speed2 < 0)
    {
        GPIO_ResetBits(MOTOR_PORT, MOTOR2_PIN1);
        GPIO_SetBits(MOTOR_PORT, MOTOR2_PIN2);
        TIM4->CCR2 = (uint16_t)(-speed2 * 1000);
    }
    else
    {
        GPIO_ResetBits(MOTOR_PORT, MOTOR2_PIN1);
        GPIO_ResetBits(MOTOR_PORT, MOTOR2_PIN2);
        TIM4->CCR2 = 0;
    }
}

void pid_control(void)
{
    pid_output = get_balance();
    set_motor(pid_output, -pid_output);
    delay_ms(10);
}

注意：该代码只是一个简单的示例，实际使用时需要根据实际情况进行调整和优化。另外，代码中还需要定义一个延时函数 `delay_ms()`，该函数可以使用定时器或者循环计数实现。