#include "stm32f10x.h"
#include "mpu6050.h"
#include "Delay.h"
#include "OLED.h"
#include <math.h>
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
// ????
float lingpiao = 0; // ???????
volatile float yaw = 0; // ??? (0~360?)
float target_yaw = 0.0f; // ?????
volatile uint32_t system_tick = 0; // ??HAL_GetTick????????
// ???PID???
typedef struct
{
// PID??
float Kp;
float Ki;
float Kd;
// ????
float last_error; // ????? e(k-1)
float prev_error; // ????? e(k-2)
// ????
float max_output; // ??????
float min_output; // ??????
// ????
float output; // ??????
} PID_IncTypeDef;
PID_IncTypeDef pid; // PID?????
/**
* @brief PID?????
*/
void PID_IncInit(PID_IncTypeDef *pid, float kp, float ki, float kd, float min_out, float max_out)
{
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
pid->min_output = min_out;
pid->max_output = max_out;
// ??????
pid->last_error = 0;
pid->prev_error = 0;
pid->output = 0;
}
/**
* @brief ???PID??
* @param target: ???
* @param feedback: ???
* @retval ??????
*/
float PID_IncCalculate(PID_IncTypeDef *pid, float target, float feedback)
{
float error, p_out, i_out, d_out;
float delta_output;
// 1. ?????? (??????)
error = target - feedback;
// ??????[-180, 180]??
if (error > 180.0f) error -= 360.0f;
else if (error < -180.0f) error += 360.0f;
// 2. ??P??
p_out = pid->Kp * (error - pid->last_error);
// 3. ??I??
i_out = pid->Ki * error;
// 4. ??D??
d_out = pid->Kd * (error - 2 * pid->last_error + pid->prev_error);
// 5. ???????
delta_output = p_out + i_out + d_out;
// 6. ??????
pid->prev_error = pid->last_error;
pid->last_error = error;
// 7. ????
if (delta_output > pid->max_output)
delta_output = pid->max_output;
else if (delta_output < pid->min_output)
delta_output = pid->min_output;
// 8. ?????
pid->output += delta_output;
// ?????
if (pid->output > pid->max_output)
pid->output = pid->max_output;
else if (pid->output < pid->min_output)
pid->output = pid->min_output;
return pid->output;
}
void Timer_Init(void) {
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_TimeBaseInitStruct.TIM_Period = 5000 - 1; // 5ms??
TIM_TimeBaseInitStruct.TIM_Prescaler = 72 - 1; // 72MHz/72 = 1MHz
TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStruct);
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
NVIC_EnableIRQ(TIM2_IRQn);
TIM_Cmd(TIM2, ENABLE);
}
int16_t MPU6050_GetGyroZ(void) {
uint8_t high = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
uint8_t low = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
return (int16_t)((high << 8) | low);
}
void TIM2_IRQHandler(void) {
static float gyro_z_prev = 0;
if (TIM_GetITStatus(TIM2, TIM_IT_Update)) {
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
system_tick++;
float gyro_z_current = (float)(MPU6050_GetGyroZ() - lingpiao) / 16.4f;
float delta_yaw = (gyro_z_prev + gyro_z_current) * 0.0025f * 0.5f;
gyro_z_prev = gyro_z_current;
yaw += delta_yaw;
yaw = fmod(yaw, 360.0f);
if (yaw < 0) yaw += 360.0f;
float control_output = PID_IncCalculate(&pid, target_yaw, yaw);
}
}
uint32_t Get_Tick(void) {
return system_tick * 5;
}
int main(void) {
Delay_init();
Delay_ms(500);
MPU6050_Init();
OLED_Init();
Timer_Init();
PID_IncInit(&pid, 1.0f, 0.05f, 0.0f, -100.0f, 100.0f);
int32_t sum = 0;
for (int i = 0; i < 100; i++) {
sum += MPU6050_GetGyroZ();
Delay_ms(10);
}
lingpiao = sum / 100.0f;
OLED_ShowString(1, 1, "Yaw:");
OLED_ShowString(2, 1, "Tar:");
OLED_ShowString(3, 1, "PID:");
target_yaw = 0.0f;
while (1) {
int integer_part = (int)yaw;
int decimal_part = (int)((yaw - integer_part) * 10);
OLED_ShowNum(1, 6, integer_part, 3);
OLED_ShowChar(1, 9, '.');
OLED_ShowNum(1, 10, decimal_part, 1);
OLED_ShowString(1, 12, "deg");
OLED_ShowNum(2, 6, (int)target_yaw, 3);
OLED_ShowString(2, 9, "deg");
OLED_ShowNum(3, 6, (int)pid.output, 3);
static uint32_t last_change = 0;
uint32_t current_tick = Get_Tick();
if (current_tick - last_change > 5000) {
last_change = current_tick;
target_yaw = (target_yaw == 0.0f) ? 90.0f : 0.0f;
}
Delay_ms(100);
}
}
让陀螺仪转动更加精确，并且让舵机转动
最新发布
本文目标是理解比例、积分和微分增益对PID控制系统性能的影响。为方便改变增益值，为GUI添加新功能，有P、I和D增益的文本输入框，增益值通过USB命令发送到EFM8，限制在0 - 255的整数。还可参考相关Scilab脚本及指定链接内容。
扫一扫
264
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
