【轻量级 PR】：add application/pid.c · Pull Request !1 · HiEuler/eulercar_controller

#include "debug.h"
#include "gpio.h"
#include "main.h"
#include "pid.h"

extern unsigned int g_TimerInterruptCount;

/* 增量式PID */
PidEulerCar g_pidEulerCarRight;
PidEulerCar g_pidEulerCarLeft;
// 前后抖动
// #define Kp 0.05
// #define Ki 0.02
// #define Kd 0.5
// #define Kp 0.03
// #define Ki 0.01
// #define Kd 0.3

// v1
// #define Kp 0.07
// #define Ki 0.002
// #define Kd 0.0

// v2 back
// #define Kp 0.48
// #define Ki 0.95
// #define Kd 0.007

// motor right
// #define KP_RIGHT 0.50
// #define KI_RIGHT 1.01
// #define KD_RIGHT 0.007

// motor left
// #define KP_LEFT 0.50
// #define KI_LEFT 1.01
// #define KD_LEFT 0.007

// motor right
//#define KP_RIGHT 0.2
//#define KI_RIGHT 1.0
//#define KD_RIGHT 0.007

// motor right
#define KP_RIGHT 0.3
#define KI_RIGHT 0.01
#define KD_RIGHT 0.0

// motor left
#define KP_LEFT 0.2
#define KI_LEFT 0.01
#define KD_LEFT 0.0

void Pid_Right_Init(void)
{
    g_pidEulerCarRight.SetSpeed = 0;
    g_pidEulerCarRight.ActualSpeed = 0;
    g_pidEulerCarRight.TargetSpeed = 0; 
    g_pidEulerCarRight.err = 0;
    g_pidEulerCarRight.i_err = 0;
    g_pidEulerCarRight.err_last = 0;
    g_pidEulerCarRight.err_next = 0;
    g_pidEulerCarRight.kp_param = KP_RIGHT;
    g_pidEulerCarRight.ki_param = KI_RIGHT;
    g_pidEulerCarRight.kd_param = KD_RIGHT;
    DBG_PRINTF("pid right init success\r\n");
}

void Pid_Left_Init(void)
{
    g_pidEulerCarLeft.SetSpeed = 0;
    g_pidEulerCarLeft.ActualSpeed = 0;
    g_pidEulerCarLeft.TargetSpeed = 0; 
    g_pidEulerCarLeft.err = 0;
    g_pidEulerCarLeft.i_err = 0;
    g_pidEulerCarLeft.err_last = 0;
    g_pidEulerCarLeft.err_next = 0;
    g_pidEulerCarLeft.kp_param = KP_LEFT;
    g_pidEulerCarLeft.ki_param = KI_LEFT;
    g_pidEulerCarLeft.kd_param = KD_LEFT;
    DBG_PRINTF("pid left init success\r\n");
}

void Set_Pid_TargertSpeed(float initSpeed)
{
    g_pidEulerCarRight.TargetSpeed = initSpeed;
    g_pidEulerCarLeft.TargetSpeed = initSpeed;
}

float Pid_Right_Ctrl(float setSpeed, float realSpeed)
{

g_pidEulerCarRight.SetSpeed = setSpeed;
    g_pidEulerCarRight.ActualSpeed = realSpeed;
    
    //位置式PID算法，计算出目标轮速
    g_pidEulerCarRight.err = g_pidEulerCarRight.SetSpeed - g_pidEulerCarRight.ActualSpeed;
    g_pidEulerCarRight.i_err = g_pidEulerCarRight.i_err + g_pidEulerCarRight.err; 
    g_pidEulerCarRight.TargetSpeed = KP_RIGHT * g_pidEulerCarRight.err 
                                   + KI_RIGHT * g_pidEulerCarRight.i_err 
                                   + KD_RIGHT * (g_pidEulerCarRight.err - g_pidEulerCarRight.err_next);

g_pidEulerCarRight.err_last = g_pidEulerCarRight.err_next;
    g_pidEulerCarRight.err_next = g_pidEulerCarRight.err;

//限制幅度
    if(setSpeed > 0){
        if (g_pidEulerCarRight.TargetSpeed > 1280){
            g_pidEulerCarRight.TargetSpeed = 1280;
        }
        if (g_pidEulerCarRight.TargetSpeed < 0){
            g_pidEulerCarRight.TargetSpeed = 0;
        }
    }

if(setSpeed < 0){
        if (g_pidEulerCarRight.TargetSpeed < -1280){
            g_pidEulerCarRight.TargetSpeed = -1280;
        }
        if (g_pidEulerCarRight.TargetSpeed > 0){
            g_pidEulerCarRight.TargetSpeed = 0;
        }
    }

//如果电机停止需要设置启动PWM基础值
    if(g_pidEulerCarRight.ActualSpeed < 1){
        if(setSpeed > 0){
            g_pidEulerCarRight.TargetSpeed = g_pidEulerCarRight.TargetSpeed + 200;
        }
        else{
            g_pidEulerCarRight.TargetSpeed = g_pidEulerCarRight.TargetSpeed - 200; 
        }
    }

//每计数10次打印1次 
    // if((g_TimerInterruptCount % 10) == 0) {
    //     DBG_PRINTF("MR:SS:%.0f, AS:%.0f, err:%.0f, TS:%.0f\r\n", 
    //                   g_pidEulerCarRight.SetSpeed, g_pidEulerCarRight.ActualSpeed,g_pidEulerCarRight.err,g_pidEulerCarRight.TargetSpeed);
    // }

return g_pidEulerCarRight.TargetSpeed;
}

float Pid_Left_Ctrl(float setSpeed, float realSpeed)
{

g_pidEulerCarLeft.SetSpeed = setSpeed;
    g_pidEulerCarLeft.ActualSpeed = realSpeed;
    
    //位置式PID算法，计算出目标轮速
    g_pidEulerCarLeft.err = g_pidEulerCarLeft.SetSpeed - g_pidEulerCarLeft.ActualSpeed;
    g_pidEulerCarLeft.i_err = g_pidEulerCarLeft.i_err + g_pidEulerCarLeft.err;

g_pidEulerCarLeft.TargetSpeed = KP_RIGHT * g_pidEulerCarLeft.err 
                                   + KI_RIGHT * g_pidEulerCarLeft.i_err 
                                   + KD_RIGHT * (g_pidEulerCarLeft.err - g_pidEulerCarLeft.err_last);

g_pidEulerCarLeft.err_last = g_pidEulerCarLeft.err_next;
    g_pidEulerCarLeft.err_next = g_pidEulerCarLeft.err;

//限制幅度
    if(setSpeed > 0){
        if (g_pidEulerCarLeft.TargetSpeed > 1280){
            g_pidEulerCarLeft.TargetSpeed = 1280;
        }
        if (g_pidEulerCarLeft.TargetSpeed < 0){
            g_pidEulerCarLeft.TargetSpeed = 0;
        }
    }

if(setSpeed < 0){
        if (g_pidEulerCarLeft.TargetSpeed < -1280){
            g_pidEulerCarLeft.TargetSpeed = -1280;
        }
        if (g_pidEulerCarLeft.TargetSpeed > 0){
            g_pidEulerCarLeft.TargetSpeed = 0;
        }
    }

//如果电机停止需要设置启动PWM基础值
    if(g_pidEulerCarLeft.ActualSpeed < 1){
        if(setSpeed > 0){
            g_pidEulerCarLeft.TargetSpeed = g_pidEulerCarLeft.TargetSpeed + 200;
        }
        else{
            g_pidEulerCarLeft.TargetSpeed = g_pidEulerCarLeft.TargetSpeed - 200; 
        }
    }

//每计数10次打印1次 
    if((g_TimerInterruptCount % 10) == 0) {
        DBG_PRINTF("ML:SS:%.0f, AS:%.0f, err:%.0f,TS:%.0f\r\n", 
                      g_pidEulerCarLeft.SetSpeed, g_pidEulerCarLeft.ActualSpeed,g_pidEulerCarLeft.err,g_pidEulerCarLeft.TargetSpeed);
    }

return g_pidEulerCarLeft.TargetSpeed;

}

类型	指派人员	状态
审查	fan	进行中 (0/1人)
测试	fan	进行中 (0/1人)

HiEuler/eulercar_controller .gitee-modal { width: 500px !important; }

搜索帮助

HiEuler/eulercar_controller