STM32 PID 控制 MPU6050 示例代码

#include "stm32f4xx.h" #include "stm32f4xx_gpio.h" #include "stm32f4xx_rcc.h" #include "stm32f4xx_usart.h" #include "stm32f4xx_i2c.h" #include "stm32f4xx_syscfg.h"

#define MPU6050_ADDRESS 0x68

#define P_GAIN 1.0f #define I_GAIN 0.0f #define D_GAIN 0.0f

float p_gain = P_GAIN; float i_gain = I_GAIN; float d_gain = D_GAIN;

float target_angle = 0.0f; float current_angle = 0.0f; float prev_angle = 0.0f;

float integral = 0.0f; float derivative = 0.0f;

float pid_output = 0.0f;

void MPU6050_Init(void); void MPU6050_Read_Accelerometer(float* x, float* y, float* z); void MPU6050_Calculate_Angle(float ax, float ay, float az, float* angle); void PID_Controller(void);

void Delay(__IO uint32_t nCount) { while(nCount--) { } }

int main(void) { MPU6050_Init();

while(1) {
    float ax, ay, az;
    MPU6050_Read_Accelerometer(&ax, &ay, &az);
    MPU6050_Calculate_Angle(ax, ay, az, &current_angle);
    PID_Controller();
    // 控制输出代码
    Delay(1000);
}

}

void MPU6050_Init(void) { I2C_InitTypeDef I2C_InitStruct; GPIO_InitTypeDef GPIO_InitStruct;

RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_9;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStruct.GPIO_OType = GPIO_OType_OD;
GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_UP;
GPIO_Init(GPIOB, &GPIO_InitStruct);

GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_I2C1);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource9, GPIO_AF_I2C1);

I2C_InitStruct.I2C_ClockSpeed = 400000;
I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStruct.I2C_OwnAddress1 = 0x00;
I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;
I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_Init(I2C1, &I2C_InitStruct);

I2C_Cmd(I2C1, ENABLE);

while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));

I2C_GenerateSTART(I2C1, ENABLE);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS << 1, I2C_Direction_Transmitter);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

I2C_SendData(I2C1, 0x6B);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

I2C_SendData(I2C1, 0);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

I2C_GenerateSTOP(I2C1, ENABLE);

}

void MPU6050_Read_Accelerometer(float* x, float* y, float* z) { I2C_GenerateSTART(I2C1, ENABLE); while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS << 1, I2C_Direction_Transmitter);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

I2C_SendData(I2C1, 0x3B);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

I2C_GenerateSTART(I2C1, ENABLE);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));

I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS << 1, I2C_Direction_Receiver);
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
*x = (float)I2C_ReceiveData(I2C1) / 16384.0f;
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
*y = (float)I2C_ReceiveData(I2C1) / 16384.0f;
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED));
*z = (float)I2C_ReceiveData(I2C1) / 16384.0f;

I2C_GenerateSTOP(I2C1, ENABLE);

}

void MPU6050_Calculate_Angle(float ax, float ay, float az, float* angle) { *angle = atan2f(ay, az) * 180.0f / 3.14159265f; }

void PID_Controller(void) { float error = target_angle - current_angle;

integral += error;
derivative = current_angle - prev_angle;

pid_output = p_gain * error + i_gain * integral + d_gain * derivative;

prev_angle = current_angle;

}