ELCT/25-sentry-chassis
6
0
Fork 0
Code Issues Pull Requests Activity

更新项目结构，修改自述文件

Browse Source
This commit is contained in:
Huang Haoyu
2026-05-17 21:16:52 +08:00
parent bc5bd0c72e
commit a4ccd91c75
28 changed files with 129 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
02-Application/Chassis_CtoC.c Normal file
View File

@@ -0,0 +1,51 @@
#include "Chassis_CtoC.h"
#include "can.h"
#include "BMI088.h"
#include "BMI088driver.h"
Remote_Data Remote_RxData;//遥控器接收数据
extern BMI088_Init_typedef BMI088_Data;
void CToC_CANDataProcess(uint32_t ID,uint8_t *Data)
{
if(ID==0x149)//接收遥控器拨杆数据
{
Remote_RxData.Remote_R_RL=(int16_t)((uint16_t)Data[0]<<8 | Data[1]);//右摇杆右左
Remote_RxData.Remote_R_UD=(int16_t)((uint16_t)Data[2]<<8 | Data[3]);//右摇杆上下
Remote_RxData.Remote_L_RL=(int16_t)((uint16_t)Data[4]<<8 | Data[5]);//左摇杆右左
Remote_RxData.Remote_L_UD=(int16_t)((uint16_t)Data[6]<<8 | Data[7]);//左摇杆上下
}
// else if(ID==0x189)//接收遥控器控制数据
// {
// Remote_Status=Data[0];//遥控器连接状态
// Remote_RxData.Remote_RS=Data[1];//遥控器右侧拨动开关
// Remote_RxData.Remote_KeyPush_Ctrl=Data[2];//键盘Ctrl状态
// Remote_RxData.Remote_KeyPush_Shift=Data[3];//键盘Shift状态
// Remote_StartFlag=Data[4];//遥控器启动标志位
// Remote_RxData.Remote_LS=Data[5];//遥控器左侧拨动开关
// }
}
void Chassis_CtoC_BMI088(BMI088_Init_typedef *data)
{
BMI088_GetData(data);
//定义缩放系数
float GYRO_SCALE = 100.0f; // rad/s → 0.01 rad/s per LSB
float ACCEL_SCALE = 100.0f; // m/s² → 0.001 m/s² per LSB
float TEMP_SCALE = 10.0f; // °C → 0.1°C per LSB
int16_t d1, d2, d3, d4;
// 3. 根据 CAN ID 决定发送内容
int16_t yaw = (int16_t)(data->Yaw * 100);
int16_t pitch = (int16_t)(data->Pitch * 100);
int16_t roll = (int16_t)(data->Roll * 10);
int16_t tmp = (int16_t)(data->Temp * TEMP_SCALE);
BMI088_Can_Angle(yaw, pitch, roll, tmp, &hcan2);
}

37
02-Application/Chassis_CtoC.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef CHASSIS_CTOC_H
#define CHASSIS_CTOC_H
#include <stdint.h>
#include "BMI088.h"
typedef struct
{
int16_t Remote_R_RL;//通道0-右摇杆左右(右为大),范围364(最左端)~1684(最右端),默认值1024(中间)
int16_t Remote_R_UD;//通道1-右摇杆上下(上为大),范围364(最下端)~1684(最上端),默认值1024(中间)
int16_t Remote_L_RL;//通道2-左摇杆左右(右为大),范围364(最左端)~1684(最右端),默认值1024(中间)
int16_t Remote_L_UD;//通道3-左摇杆上下(上为大),范围364(最下端)~1684(最上端),默认值1024(中间)
uint8_t Remote_LS;//S1-左侧拨动开关,范围1~3,上为1,下为2,中间为3
uint8_t Remote_RS;//S2-右侧拨动开关,范围1~3,上为1,下为2,中间为3
int16_t Remote_Mouse_RL;//鼠标X轴-鼠标左右速度,范围-32768~32767,向右为正,向左为负,静止值为0
int16_t Remote_Mouse_DU;//鼠标Y轴-鼠标前后速度,范围-32768~32767,向后为正,向前为负,静止值为0
int16_t Remote_Mouse_Wheel;//鼠标Z轴-鼠标滚轮速度,范围-32768~32767,向前为正,向后为负,静止值为0
uint8_t Remote_Mouse_KeyL;//鼠标左键,按下为1,未按下为0
uint8_t Remote_Mouse_KeyR;//鼠标右键,按下为1,未按下为0
uint8_t Remote_Key_W;//键盘W键,按下为1,未按下为0
uint8_t Remote_Key_S;//键盘S键,按下为1,未按下为0
uint8_t Remote_Key_A;//键盘A键,按下为1,未按下为0
uint8_t Remote_Key_D;//键盘D键,按下为1,未按下为0
uint8_t Remote_Key_Q;//键盘Q键,按下为1,未按下为0
uint8_t Remote_Key_E;//键盘E键,按下为1,未按下为0
uint8_t Remote_Key_Shift;//键盘Shift键,按下为1,未按下为0
uint8_t Remote_Key_Ctrl;//键盘Ctrl键,按下为1,未按下为0
uint8_t Remote_KeyPush_Ctrl;//按下键盘Ctrl键,按下时0,1切换
uint8_t Remote_KeyPush_Shift;//按下键盘Shift键,按下时0,1切换
int16_t Remote_ThumbWheel;//保留字段-遥控器拨轮,范围-3278(最上端)~1684(最下端),默认值1024
}Remote_Data;//遥控器接收结构体
void CToC_CANDataProcess(uint32_t ID,uint8_t *Data);
void Chassis_CtoC_BMI088(BMI088_Init_typedef *data);
#endif //CHASSIS_CTOC_H

93
02-Application/Chassis_Yaw_Big.c Normal file
View File

@@ -0,0 +1,93 @@
//#include "Chassis_Yaw_Big.h"
//PID_PositionInitTypedef BigYaw_PositionPID;
//PID_PositionInitTypedef BigYaw_SpeedPID;
//extern motor_measure_t Can1_Rx_Data[7];
//extern motor_measure_t Can2_Rx_Data[7];
//extern RC_ctrl_t *local_rc_ctrl;
//extern CAN_HandleTypeDef hcan1;
//extern CAN_HandleTypeDef hcan2;
//void Classis_YawBig_Init(void)
//{
// PID_PositionStructureInit (&BigYaw_PositionPID,0); //外环位置环
// PID_PositionSetParameter (&BigYaw_PositionPID,0.4,0,8);
// PID_PositionSetOUTRange (&BigYaw_PositionPID,-20000,20000);
// // PID_PositionSetNeedValueRange(&BigYaw_PositionPID,4848,0);
// PID_PositionStructureInit (&BigYaw_SpeedPID,0); //内环速度环
// PID_PositionSetParameter (&BigYaw_SpeedPID,100,0,0);
// PID_PositionSetOUTRange (&BigYaw_SpeedPID,-15000,15000);
//}
//void Classis_YawBig_Control(void)
//{
// // ============速度环pid调节代码============
//// static uint32_t tick = 0;
//// static int i = 0;
//// float target_speed = 0.0f;
//// // 每 1000ms 切换一次状态（1秒）
//// if (HAL_GetTick() - tick > 1000) {
//// tick = HAL_GetTick();
//// i++;
//// }
//// // i=0: 0 RPM, i=1: +100, i=2: 0, i=3: -100, 然后循环
//// switch (i % 4) {
//// case 0: target_speed = 0.0f; break; // 停
//// case 1: target_speed = 60.0f; break; // 正转
//// case 2: target_speed = 0.0f; break; // 停
//// case 3: target_speed = 60.0f; break; // 反转
//// }
////
////
//// PID_PositionSetNeedValue(&BigYaw_SpeedPID, target_speed);
//// PID_PositionCalc(&BigYaw_SpeedPID, Can2_Rx_Data[4].speed_rpm);
////
//// Motor_6020_Voltage1((int16_t)BigYaw_SpeedPID.OUT, 0, 0, 0, &hcan2);
// // ========================
//
//
//
//
//
//
// #define RC_DEADBAND 10
// float target_speed = 0.0f;
// int16_t ch0 = local_rc_ctrl->rc.ch[0];
// // ============ 1. 更新位置目标（仅打杆时）============
// if (ch0 > RC_DEADBAND || ch0 < -RC_DEADBAND) {
// BigYaw_PositionPID.Need_Value -= 0.015f * ch0;
// if (BigYaw_PositionPID.Need_Value > 8191.0f)
// BigYaw_PositionPID.Need_Value -= 8192.0f;
// else if (BigYaw_PositionPID.Need_Value < 0.0f)
// BigYaw_PositionPID.Need_Value += 8192.0f;
// }
//// BigYaw_PositionPID.Need_Value = 2048.0f;
// // ===================================================
// // ============ 2. 位置环计算 =========================
// PID_PositionCalc_Motor(&BigYaw_PositionPID, Can2_Rx_Data[4].ecd);
// // ===================================================
// // ============ 3 =====
//// if (ch0 > RC_DEADBAND || ch0 < -RC_DEADBAND) {
// target_speed = BigYaw_PositionPID.OUT;
//// }
// // ===================================================
// // ============ 4. 速度环计算 =========================
// BigYaw_SpeedPID.Need_Value = target_speed;
// PID_PositionCalc(&BigYaw_SpeedPID, Can2_Rx_Data[4].speed_rpm);
// // ===================================================
// // ============ 5. 发送输出 ===========================
// Motor_6020_Voltage1((int16_t)BigYaw_SpeedPID.OUT, 0, 0, 0, &hcan2);
//
//}

10
02-Application/Chassis_Yaw_Big.h Normal file
View File

@@ -0,0 +1,10 @@
//#ifndef CLASSIS_YAW_BIG_H
//#define CLASSIS_YAW_BIG_H
//#include "PID.h"
//#include "BSP_CAN.h"
//#include "Remote.h"
//#include "Motor.h"
//void Classis_YawBig_Init(void);
//void Classis_YawBig_Control(void);
//#endif // __GIMBAL_YAW_SMALL_H

176
02-Application/Classis_Wheels.c Normal file
View File

@@ -0,0 +1,176 @@
#include "Classis_Wheels.h"
#include "can.h"
#include "Parameter.h"
#include <math.h>
PID_PositionInitTypedef Wheels_SpeedPID[4];
extern M3508_Motor Can1_M3508_MotorStatus[8];
extern M3508_Motor Can2_M3508_MotorStatus[8];
extern M6020_Motor Can1_M6020_MotorStatus[7];//GM6020电机状态数组
extern M6020_Motor Can2_M6020_MotorStatus[7];//GM6020电机状态数组
extern Remote_Data Remote_RxData;//遥控器接收数据
extern CAN_HandleTypeDef hcan1;
//-------------------测试区
#define BigYaw_MediumPosition -114//大yaw中位位置(直接读大yaw电机角度)
#define Mecanum_rx 0.24f //底盘中心到轮子中心的距离的x轴分量(m)
#define Mecanum_ry 0.24f //底盘中心到轮子中心的距离的y轴分量(m)
#define Mecanum_WheelRadius 0.07f //轮子半径(m)
#define MAX_RPM 2000 // 最大转速，根据电机调整
#define L 0.2f // 轮距（单位：米）
float K_yaw = 10.0f; // 旋转速度的比例系数
//#define Wheel_RightFront 0x201//右前轮
//#define Wheel_LeftFront 0x202//左前轮
//#define Wheel_LeftRear 0x203//左后轮
//#define Wheel_RightRear 0x204//右后轮
void Classis_Wheels_Init(void)
{
PID_PositionStructureInit (&Wheels_SpeedPID[0],0);
PID_PositionSetParameter (&Wheels_SpeedPID[0],10,0,0);
PID_PositionSetOUTRange (&Wheels_SpeedPID[0],-20000,20000);
PID_PositionSetEkRange (&Wheels_SpeedPID[0], -3.0f, 3.0f);
PID_PositionStructureInit (&Wheels_SpeedPID[1],0);
PID_PositionSetParameter (&Wheels_SpeedPID[1],10,0,0);
PID_PositionSetOUTRange (&Wheels_SpeedPID[1],-20000,20000);
PID_PositionSetEkRange (&Wheels_SpeedPID[1], -3.0f, 3.0f);
PID_PositionStructureInit (&Wheels_SpeedPID[2],0);
PID_PositionSetParameter (&Wheels_SpeedPID[2],10,0,0);
PID_PositionSetOUTRange (&Wheels_SpeedPID[2],-20000,20000);
PID_PositionSetEkRange (&Wheels_SpeedPID[2],-3.0f, 3.0f);
PID_PositionStructureInit (&Wheels_SpeedPID[3],0);
PID_PositionSetParameter (&Wheels_SpeedPID[3],10,0,0);
PID_PositionSetOUTRange (&Wheels_SpeedPID[3],-20000,20000);
PID_PositionSetEkRange (&Wheels_SpeedPID[3], -3.0f, 3.0f);
// PID_PositionStructureInit(&Mecanum_TrackPID,4096);
// PID_PositionSetParameter(&Mecanum_TrackPID,0,0,0);
// PID_PositionSetEkRange(&Mecanum_TrackPID,-5,5);
// PID_PositionSetOUTRange(&Mecanum_TrackPID,-1,1);
}
//-------------------测试区
// void Classis_Wheels_Control(void)
// {
// extern const RC_ctrl_t *local_rc_ctrl;
// // 1. 获取左摇杆（ch[2]=左右, ch[3]=上下）
// float vy_chassis = -(float)Remote_RxData.Remote_R_RL; // 左右 → 横向
// float vx = -(float)Remote_RxData.Remote_R_UD; // 上下 → 纵向
// // 3. 麦轮解算 —— 用一个缩放系数 K 代替所有几何参数
// const float K = 4.0f;
// int16_t fl = (int16_t)((vx - vy_chassis) * K); // 左前
// int16_t fr = (int16_t)((vx + vy_chassis) * K); // 右前
// int16_t bl = (int16_t)((vx + vy_chassis) * K); // 左后
// int16_t br = (int16_t)((vx - vy_chassis) * K); // 右后
// // 4. 发送目标转速（单位：RPM）
// Classis_Wheels_Setspeed(fl, fr, bl, br);
// }
/*-------------------测试区
↑y
|
------------------>x ↑w
|
|
-------------------测试区*/
void Classis_Wheels_Control(void)
{
// 1. 通过遥控器获取车体运动的期望速度:x轴前后平动速度,y轴左右平动速度,转动角速度(前右为正,逆时针为正)
float vy_chassis = (float)Remote_RxData.Remote_R_UD / 660.0f;//范围-1~1，归一化为-1~1的速度比例
float vx_chassis = (float)Remote_RxData.Remote_R_RL / 660.0f;
float sigma = sqrtf(vx_chassis*vx_chassis + vy_chassis*vy_chassis);//归一化系数
float w_chassis = 0.0f; // 旋转速度暂时设为3
float theta = BigYaw_MediumPosition - Can2_M6020_MotorStatus[0].ANgle; //云台底盘相对角度
if(theta > 180.0f) theta -= 360.0f; // 将角度调整到[-180, 180]范围内
if(theta < -180.0f) theta += 360.0f;
vy_chassis = vy_chassis / sigma; // 归一化到最大转速
vx_chassis = vx_chassis / sigma; // 归一化到最大转速
float vx_=vx_chassis,vy_=vy_chassis;
vy_chassis = vy_*cosf(theta)-vx_*sinf(theta);//根据底盘云台相对角度修正xy轴速度
vx_chassis = vx_*cosf(theta)+vy_*sinf(theta);
//2.
vy_chassis *= 0.2;
vx_chassis *= 0.2;
int16_t RightFrontSpeed =(int16_t)((-vx_chassis+vy_chassis+w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//右前
int16_t LeftFrontSpeed =(int16_t)((+vx_chassis+vy_chassis-w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//左前
int16_t LeftRearSpeed =(int16_t)((-vx_chassis+vy_chassis-w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//左后
int16_t RightRearSpeed =(int16_t)((+vx_chassis+vy_chassis+w_chassis*(Mecanum_rx+Mecanum_ry))/Mecanum_WheelRadius*181.43663512f);//右后
//w' = v/R (rad/s) = v/(2Π×R) (r/s)(圈每秒) = 60×v/(2Π×R) (r/min)=1/19 w
//⇒ w=19 × 60 × v/(2Π×R)=19×60×v/(2Π×R)=181.43663512×v/R (圈每分),R单位m
Classis_Wheels_Setspeed(RightFrontSpeed,LeftFrontSpeed,LeftRearSpeed,RightRearSpeed);
}
void Classis_Wheels_Setspeed(int16_t LeftFrontSpeed,int16_t RightFrontSpeed,int16_t LeftRearSpeed,int16_t RightRearSpeed)
{
//更改期望
Wheels_SpeedPID[0].Need_Value =LeftFrontSpeed;//左前轮
Wheels_SpeedPID[1].Need_Value =RightFrontSpeed;//右前轮;
Wheels_SpeedPID[2].Need_Value =LeftRearSpeed;//左后轮
Wheels_SpeedPID[3].Need_Value =RightRearSpeed;//右后轮
//PID计算
PID_PositionCalc(&Wheels_SpeedPID[0],Can1_M3508_MotorStatus[0].RotorSpeed);//右前轮
PID_PositionCalc(&Wheels_SpeedPID[1],Can1_M3508_MotorStatus[1].RotorSpeed);//左前轮
PID_PositionCalc(&Wheels_SpeedPID[2],Can1_M3508_MotorStatus[2].RotorSpeed);//左后轮
PID_PositionCalc(&Wheels_SpeedPID[3],Can1_M3508_MotorStatus[3].RotorSpeed);//右后轮
// Mecanum_Current=0;
// for(uint8_t i=0;i<4;i++)
// Mecanum_Current+=fabs(Wheels_SpeedPID[i].OUT);
// if(Mecanum_Current>Mecanum_CurrentLimit)
// {
// Wheels_SpeedPID[0].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
// Wheels_SpeedPID[1].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
// Wheels_SpeedPID[2].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
// Wheels_SpeedPID[3].OUT*=(Mecanum_CurrentLimit/Mecanum_Current);
// }
//
Motor_3508_Current1((int16_t)Wheels_SpeedPID[0].OUT , (int16_t)Wheels_SpeedPID[1].OUT,
(int16_t)Wheels_SpeedPID[2].OUT , (int16_t)Wheels_SpeedPID[3].OUT,
&hcan1);//M3508控制
}

17
02-Application/Classis_Wheels.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef CLASSIS_WHEEL_H
#define CLASSIS_WHEEL_H
#include "PID.h"
#include "BSP_CAN.h"
#include "Remote.h"
#include "Motor.h"
#include "Chassis_CtoC.h"
#define Wheel_RightFront 0x201//右前轮
#define Wheel_LeftFront 0x202//左前轮
#define Wheel_LeftRear 0x203//左后轮
#define Wheel_RightRear 0x204//右后轮
void Classis_Wheels_Init(void);
void Classis_Wheels_Control(void);
void Classis_Wheels_Setspeed(int16_t LeftFrontSpeed,int16_t RightFrontSpeed,int16_t LeftRearSpeed,int16_t RightRearSpeed);//更改needvalue
#endif // __GIMBAL_YAW_SMALL_H

262
02-Application/PID.c Normal file
View File

@@ -0,0 +1,262 @@
#include "PID.h"
#define Motor_ECD_MAX 8192.0f
/*
*函数简介:位置式PID初始化结构体
*参数说明:位置式PID参数结构体
*参数说明:预期值
*返回类型:无
*备注:无
*/
void PID_PositionStructureInit(PID_PositionInitTypedef* PID_InitStructure,float NeedValue)
{
PID_InitStructure->Need_Value=NeedValue;
PID_InitStructure->Ek=0;
PID_InitStructure->Ek_Sum=0;
PID_InitStructure->Ek_Min=0;
PID_InitStructure->Ek_Max=0;
PID_InitStructure->Kp=0;
PID_InitStructure->Ki=0;
PID_InitStructure->Kd=0;
PID_InitStructure->OUT_Min=0;
PID_InitStructure->OUT_Max=0;
}
/*
*函数简介:位置式PID设置参数
*参数说明:位置式PID参数结构体
*参数说明:单精度浮点型Kp
*参数说明:单精度浮点型Ki
*参数说明:单精度浮点型Kd
*返回类型:无
*备注:无
*/
void PID_PositionSetParameter(PID_PositionInitTypedef* PID_InitStructure,float kp,float ki,float kd)
{
PID_InitStructure->Kp=kp;
PID_InitStructure->Ki=ki;
PID_InitStructure->Kd=kd;
}
/*
*函数简介:位置式PID设置误差为0阈值
*参数说明:位置式PID参数结构体
*参数说明:误差为0阈值下限
*参数说明:误差为0阈值上限
*返回类型:无
*备注:无
*/
void PID_PositionSetEkRange(PID_PositionInitTypedef* PID_InitStructure,float Ek_Min,float Ek_Max)
{
PID_InitStructure->Ek_Min=Ek_Min;
PID_InitStructure->Ek_Max=Ek_Max;
}
/*
*函数简介:位置式PID设置输出限幅
*参数说明:位置式PID参数结构体
*参数说明:输出限幅下限
*参数说明:输出限幅上限
*返回类型:无
*备注:无
*/
void PID_PositionSetOUTRange(PID_PositionInitTypedef* PID_InitStructure,float OUT_Min,float OUT_Max)
{
PID_InitStructure->OUT_Min=OUT_Min;
PID_InitStructure->OUT_Max=OUT_Max;
}
void PID_PositionSetNeedValue(PID_PositionInitTypedef* PID_InitStructure,float NeedValue)
{
PID_InitStructure->Need_Value=NeedValue;
}
/*
*函数简介:位置式PID清理
*参数说明:位置式PID参数结构体
*返回类型:无
*备注:使Ek和Sum为0
*/
void PID_PositionClean(PID_PositionInitTypedef* PID_InitStructure)
{
PID_InitStructure->Ek=0;
PID_InitStructure->Ek_Sum=0;
}
/*
*函数简介:位置式PID计算
*参数说明:位置式PID参数结构体
*参数说明:当前值
*返回类型:无
*备注:OUT=POUT+IOUT+DOUT=Kp*Ek+Ki*ΣEk+Kd*(Ek-Ek_Last)
*备注:计算结果保存在位置式PID参数结构体中
*/
void PID_PositionCalc(PID_PositionInitTypedef* PID_InitStructure,float NowValue)
{
PID_InitStructure->Now_Value = NowValue;
PID_InitStructure->Ek_Last = PID_InitStructure->Ek;
PID_InitStructure->Ek = PID_InitStructure->Need_Value - PID_InitStructure->Now_Value;
if(PID_InitStructure->Ek_Min < PID_InitStructure->Ek && PID_InitStructure->Ek < PID_InitStructure->Ek_Max)//误差为0检测
{
PID_InitStructure->Ek = 0;
}
PID_InitStructure->Ek_Sum += PID_InitStructure->Ek;
PID_InitStructure->Ek_Del = PID_InitStructure->Ek - PID_InitStructure->Ek_Last;//误差差分
if (PID_InitStructure->Ek_Sum > PID_InitStructure->OUT_Max)
PID_InitStructure->Ek_Sum = PID_InitStructure->OUT_Max;
else if (PID_InitStructure->Ek_Sum < PID_InitStructure->OUT_Min)
PID_InitStructure->Ek_Sum = PID_InitStructure->OUT_Min;
PID_InitStructure->P_OUT = PID_InitStructure->Kp * PID_InitStructure->Ek;
PID_InitStructure->I_OUT = PID_InitStructure->Ki * PID_InitStructure->Ek_Sum;
PID_InitStructure->D_OUT = PID_InitStructure->Kd * PID_InitStructure->Ek_Del;
PID_InitStructure->OUT = PID_InitStructure->P_OUT + PID_InitStructure->I_OUT + PID_InitStructure->D_OUT;
if(PID_InitStructure->OUT<PID_InitStructure->OUT_Min)//输出限幅
PID_InitStructure->OUT = PID_InitStructure->OUT_Min;
if(PID_InitStructure->OUT>PID_InitStructure->OUT_Max)
PID_InitStructure->OUT = PID_InitStructure->OUT_Max;
}
/*
*函数简介:位置式PID计算
*参数说明:位置式PID参数结构体
*参数说明:当前值
*返回类型:无
*备注:OUT=POUT+IOUT+DOUT=Kp*Ek+Ki*ΣEk+Kd*(Ek-Ek_Last)
*备注:计算结果保存在位置式PID参数结构体中
*/
void PID_PositionCalc_Motor(PID_PositionInitTypedef* PID_InitStructure,float NowValue)
{
PID_InitStructure->Now_Value = NowValue;
float err = PID_InitStructure->Need_Value - PID_InitStructure->Now_Value;
if (err > 8192/2) { // 8192 / 2 = 4096
err -= 8192;
} else if (err < -8192/2) {
err += 8192;
}
PID_InitStructure->Ek_Last = PID_InitStructure->Ek;
PID_InitStructure->Ek = err;
if(PID_InitStructure->Ek_Min < PID_InitStructure->Ek && PID_InitStructure->Ek < PID_InitStructure->Ek_Max)//误差为0检测
{
PID_InitStructure->Ek = 0;
}
PID_InitStructure->Ek_Sum += PID_InitStructure->Ek;
PID_InitStructure->Ek_Del = PID_InitStructure->Ek - PID_InitStructure->Ek_Last;//误差差分
if (PID_InitStructure->Ek_Sum > PID_InitStructure->OUT_Max)
PID_InitStructure->Ek_Sum = PID_InitStructure->OUT_Max;
else if (PID_InitStructure->Ek_Sum < PID_InitStructure->OUT_Min)
PID_InitStructure->Ek_Sum = PID_InitStructure->OUT_Min;
PID_InitStructure->P_OUT = PID_InitStructure->Kp * PID_InitStructure->Ek;
PID_InitStructure->I_OUT = PID_InitStructure->Ki * PID_InitStructure->Ek_Sum;
PID_InitStructure->D_OUT = PID_InitStructure->Kd * PID_InitStructure->Ek_Del;
PID_InitStructure->OUT = PID_InitStructure->P_OUT + PID_InitStructure->I_OUT + PID_InitStructure->D_OUT;
if(PID_InitStructure->OUT<PID_InitStructure->OUT_Min)//输出限幅
PID_InitStructure->OUT = PID_InitStructure->OUT_Min;
if(PID_InitStructure->OUT>PID_InitStructure->OUT_Max)
PID_InitStructure->OUT = PID_InitStructure->OUT_Max;
}
/*
*函数简介:增量式PID初始化结构体
*参数说明:增量式PID参数结构体
*参数说明:预期值
*返回类型:无
*备注:无
*/
void PID_IncrementalStructureInit(PID_IncrementalInitTypedef* PID_InitStructure,float NeedValue)
{
PID_InitStructure->Need_Value=NeedValue;
PID_InitStructure->Ek=0;
PID_InitStructure->Ek_Last=0;
PID_InitStructure->Ek_Min=0;
PID_InitStructure->Ek_Max=0;
PID_InitStructure->Kp=0;
PID_InitStructure->Ki=0;
PID_InitStructure->Kd=0;
PID_InitStructure->OUT_Min=-1e10;
PID_InitStructure->OUT_Max=1e10;
}
/*
*函数简介:增量式PID设置参数
*参数说明:增量式PID参数结构体
*参数说明:单精度浮点型Kp
*参数说明:单精度浮点型Ki
*参数说明:单精度浮点型Kd
*返回类型:无
*备注:无
*/
void PID_IncrementalSetParameter(PID_IncrementalInitTypedef* PID_InitStructure,float kp,float ki,float kd)
{
PID_InitStructure->Kp=kp;
PID_InitStructure->Ki=ki;
PID_InitStructure->Kd=kd;
}
/*
*函数简介:增量式PID设置误差为0阈值
*参数说明:增量式PID参数结构体
*参数说明:误差为0阈值下限
*参数说明:误差为0阈值上限
*返回类型:无
*备注:无
*/
void PID_IncrementalSetEkRange(PID_IncrementalInitTypedef* PID_InitStructure,float Ek_Min,float Ek_Max)
{
PID_InitStructure->Ek_Min=Ek_Min;
PID_InitStructure->Ek_Max=Ek_Max;
}
/*
*函数简介:增量式PID设置输出限幅
*参数说明:增量式PID参数结构体
*参数说明:输出限幅下限
*参数说明:输出限幅上限
*返回类型:无
*备注:无
*/
void PID_IncrementalSetOUTRange(PID_IncrementalInitTypedef* PID_InitStructure,float OUT_Min,float OUT_Max)
{
PID_InitStructure->OUT_Min=OUT_Min;
PID_InitStructure->OUT_Max=OUT_Max;
}
/*
*函数简介:增量式PID计算
*参数说明:增量式PID参数结构体
*参数说明:当前值
*返回类型:无
*备注:OUT=POUT+IOUT+DOUT=Kp*ΔEk+Ki*ΣΔEk+Kd*(ΔEk-ΔEk_Last)=Kp*(Ek-Ek_Last)+Ki*Ek+Kd*(Ek-2*Ek_Last+Ek_Last2)
*备注:计算结果保存在增量式PID参数结构体中
*/
void PID_IncrementalCalc(PID_IncrementalInitTypedef* PID_InitStructure,float NowValue)
{
PID_InitStructure->Now_Value =NowValue;
PID_InitStructure->Ek_Last2 =PID_InitStructure->Ek_Last;
PID_InitStructure->Ek_Last =PID_InitStructure->Ek;
PID_InitStructure->Ek =PID_InitStructure->Need_Value-PID_InitStructure->Now_Value;
if(PID_InitStructure->Ek_Min < PID_InitStructure->Ek && PID_InitStructure->Ek < PID_InitStructure->Ek_Max)//误差为0检测
PID_InitStructure->Ek =0;
PID_InitStructure->P_OUT =PID_InitStructure->Kp * (PID_InitStructure->Ek - PID_InitStructure->Ek_Last);
PID_InitStructure->I_OUT =PID_InitStructure->Ki * PID_InitStructure->Ek;
PID_InitStructure->D_OUT =PID_InitStructure->Kd * (PID_InitStructure->Ek-2*PID_InitStructure->Ek_Last+PID_InitStructure->Ek_Last2);
PID_InitStructure->OUT =PID_InitStructure->P_OUT + PID_InitStructure->I_OUT+PID_InitStructure->D_OUT;
if(PID_InitStructure->OUT<PID_InitStructure->OUT_Min)//输出限幅
PID_InitStructure->OUT=PID_InitStructure->OUT_Min;
if(PID_InitStructure->OUT>PID_InitStructure->OUT_Max)
PID_InitStructure->OUT=PID_InitStructure->OUT_Max;
}

67
02-Application/PID.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef __PID_H__
#define __PID_H__
typedef struct
{
float Need_Value;//预期值
float Now_Value;//当前值
float OUT;//总输出
float OUT_Min;//输出限幅下限
float OUT_Max;//输出限幅上限
float Ek;//本次误差
float Ek_Last;//上一次误差
float Ek_Sum;//误差积分
float Kp;//Kp
float Ki;//Ki
float Kd;//Kd
float Ek_Del;//误差差分
float Ek_Min;//误差为0阈值下限
float Ek_Max;//误差为0阈值上限
float P_OUT;//比例输出
float I_OUT;//积分输出
float D_OUT;//微分输出
}PID_PositionInitTypedef;//位置式PID参数结构体
typedef struct
{
float Need_Value;//预期值
float Now_Value;//当前值
float Ek;//本次误差
float Ek_Last;//上一次误差
float Ek_Last2;//上两次误差
float Ek_Min;//误差为0阈值下限
float Ek_Max;//误差为0阈值上限
float Kp;//Kp
float Ki;//Ki
float Kd;//Kd
float P_OUT;//比例输出
float I_OUT;//积分输出
float D_OUT;//微分输出
float OUT;//总输出
float OUT_Min;//输出限幅下限
float OUT_Max;//输出限幅上限
}PID_IncrementalInitTypedef;//增量式PID参数结构体
void PID_PositionStructureInit (PID_PositionInitTypedef* PID_InitStructure,float NeedValue); //位置式PID初始化结构体
void PID_PositionSetParameter (PID_PositionInitTypedef* PID_InitStructure,float kp,float ki,float kd); //位置式PID设置参数
void PID_PositionSetEkRange (PID_PositionInitTypedef* PID_InitStructure,float ek_low,float ek_up); //位置式PID设置误差为0阈值
void PID_PositionSetOUTRange (PID_PositionInitTypedef* PID_InitStructure,float out_low,float out_up); //位置式PID设置输出限幅
void PID_PositionSetNeedValue (PID_PositionInitTypedef* PID_InitStructure,float NeedValue);
void PID_PositionClean (PID_PositionInitTypedef* PID_InitStructure); //位置式PID清理
void PID_PositionCalc (PID_PositionInitTypedef* PID_InitStructure,float NowValue); //位置式PID计算
void PID_PositionCalc_Motor (PID_PositionInitTypedef* PID_InitStructure,float NowValue);
void PID_IncrementalStructureInit (PID_IncrementalInitTypedef* PID_InitStructure,float NeedValue); //增量式PID初始化结构体
void PID_IncrementalSetParameter (PID_IncrementalInitTypedef* PID_InitStructure,float kp,float ki,float kd); //增量式PID设置参数
void PID_IncrementalSetEkRange (PID_IncrementalInitTypedef* PID_InitStructure,float ek_low,float ek_up); //增量式PID设置误差为0阈值
void PID_IncrementalSetOUTRange (PID_IncrementalInitTypedef* PID_InitStructure,float out_low,float out_up); //增量式PID设置输出限幅
void PID_IncrementalCalc (PID_IncrementalInitTypedef* PID_InitStructure,float NowValue); //增量式PID计算
#endif