大一寒假——pid在舵機與陀螺儀控制雲臺穩定——keil5
阿新 • • 發佈：2022-01-15
/** ****************************(C) COPYRIGHT 2019 DJI**************************** * @file pid.c/h * @brief pid實現函式，包括初始化，PID計算函式， * @note * @history * Version Date Author Modification * V1.0.0 Dec-26-2018 RM 1. 完成 * @verbatim ============================================================================== ============================================================================== @endverbatim ****************************(C) COPYRIGHT 2019 DJI*****************************/ #include "pid.h" #include "main.h" #define LimitMax(input, max) \ { \ if (input > max) \ { \ input = max; \ } \ else if (input < -max) \ { \ input= -max; \ } \ } /** * @brief pid struct data init * @param[out] pid: PID struct data point * @param[in] mode: PID_POSITION: normal pid * PID_DELTA: delta pid * @param[in] PID: 0: kp, 1: ki, 2:kd * @param[in] max_out: pid max out * @param[in] max_iout: pid max iout * @retval none*/ /** * @brief pid struct data init * @param[out] pid: PID結構資料指標 * @param[in] mode: PID_POSITION:普通PID * PID_DELTA: 差分PID * @param[in] PID: 0: kp, 1: ki, 2:kd * @param[in] max_out: pid最大輸出 * @param[in] max_iout: pid最大積分輸出 * @retval none */ void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout) { if (pid == NULL || PID == NULL) { return; } pid->mode = mode; pid->Kp = PID[0]; pid->Ki = PID[1]; pid->Kd = PID[2]; pid->max_out = max_out; pid->max_iout = max_iout; pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f; pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f; } /** * @brief pid calculate * @param[out] pid: PID struct data point * @param[in] ref: feedback data * @param[in] set: set point * @retval pid out */ /** * @brief pid計算 * @param[out] pid: PID結構資料指標 * @param[in] ref: 反饋資料 * @param[in] set: 設定值 * @retval pid輸出 */ fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set) { if (pid == NULL) { return 0.0f; } pid->error[2] = pid->error[1]; pid->error[1] = pid->error[0]; pid->set = set; pid->fdb = ref; pid->error[0] = set - ref; if (pid->mode == PID_POSITION) { pid->Pout = pid->Kp * pid->error[0]; pid->Iout += pid->Ki * pid->error[0]; pid->Dbuf[2] = pid->Dbuf[1]; pid->Dbuf[1] = pid->Dbuf[0]; pid->Dbuf[0] = (pid->error[0] - pid->error[1]); pid->Dout = pid->Kd * pid->Dbuf[0]; LimitMax(pid->Iout, pid->max_iout); pid->out = pid->Pout + pid->Iout + pid->Dout; LimitMax(pid->out, pid->max_out); } else if (pid->mode == PID_DELTA) { pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]); pid->Iout = pid->Ki * pid->error[0]; pid->Dbuf[2] = pid->Dbuf[1]; pid->Dbuf[1] = pid->Dbuf[0]; pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]); pid->Dout = pid->Kd * pid->Dbuf[0]; pid->out += pid->Pout + pid->Iout + pid->Dout; LimitMax(pid->out, pid->max_out); } return pid->out; } /** * @brief pid out clear * @param[out] pid: PID struct data point * @retval none */ /** * @brief pid 輸出清除 * @param[out] pid: PID結構資料指標 * @retval none */ void PID_clear(pid_type_def *pid) { if (pid == NULL) { return; } pid->error[0] = pid->error[1] = pid->error[2] = 0.0f; pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f; pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f; pid->fdb = pid->set = 0.0f; }
pid.c
#ifndef STRUCT_TYPEDEF_H #define STRUCT_TYPEDEF_H typedef signed char int8_t; typedef signed short int int16_t; typedef signed int int32_t; typedef signed long long int64_t; /* exact-width unsigned integer types */ typedef unsigned char uint8_t; typedef unsigned short int uint16_t; typedef unsigned int uint32_t; typedef unsigned long long uint64_t; typedef unsigned char bool_t; typedef float fp32; typedef double fp64; #endif
struct_typedef.h
/** ****************************(C) COPYRIGHT 2016 DJI**************************** * @file pid.c/h * @brief pid實現函式，包括初始化，PID計算函式， * @note * @history * Version Date Author Modification * V1.0.0 Dec-26-2018 RM 1. 完成 * @verbatim ============================================================================== ============================================================================== @endverbatim ****************************(C) COPYRIGHT 2016 DJI**************************** */ #ifndef PID_H #define PID_H #include "struct_typedef.h" enum PID_MODE { PID_POSITION = 0, PID_DELTA }; typedef struct { uint8_t mode; //PID 三引數 fp32 Kp; fp32 Ki; fp32 Kd; fp32 max_out; //最大輸出 fp32 max_iout; //最大積分輸出 fp32 set; fp32 fdb; fp32 out; fp32 Pout; fp32 Iout; fp32 Dout; fp32 Dbuf[3]; //微分項 0最新 1上一次 2上上次 fp32 error[3]; //誤差項 0最新 1上一次 2上上次 } pid_type_def; /** * @brief pid struct data init * @param[out] pid: PID struct data point * @param[in] mode: PID_POSITION: normal pid * PID_DELTA: delta pid * @param[in] PID: 0: kp, 1: ki, 2:kd * @param[in] max_out: pid max out * @param[in] max_iout: pid max iout * @retval none */ /** * @brief pid struct data init * @param[out] pid: PID結構資料指標 * @param[in] mode: PID_POSITION:普通PID * PID_DELTA: 差分PID * @param[in] PID: 0: kp, 1: ki, 2:kd * @param[in] max_out: pid最大輸出 * @param[in] max_iout: pid最大積分輸出 * @retval none */ extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout); /** * @brief pid calculate * @param[out] pid: PID struct data point * @param[in] ref: feedback data * @param[in] set: set point * @retval pid out */ /** * @brief pid計算 * @param[out] pid: PID結構資料指標 * @param[in] ref: 反饋資料 * @param[in] set: 設定值 * @retval pid輸出 */ extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set); /** * @brief pid out clear * @param[out] pid: PID struct data point * @retval none */ /** * @brief pid 輸出清除 * @param[out] pid: PID結構資料指標 * @retval none */ extern void PID_clear(pid_type_def *pid); #endif
pid.h
上述為一個反饋的pid程式碼
此為模擬pid輸出網站
PID online simulator - STorM32-BGC Wiki (olliw.eu)
下述為假期期間除錯所遇到的問題（針對pid和舵機程式的）
1.舵機旋轉需時間，兩次過近的命令會導致輸出無效。故主程式while(1)中應有delay（ms）以保證有時間轉動
2.pid應先從kp開始逐一調參，第一個數值應大於10，以保證有有效輸出
————————————————
參考資料原文連結：