平衡车 -- 速度环 - 详解

个人主页：羽晨同学

个人格言:“成为自己未来的主人~”

速度环

首先，我们创建一个速度环的PID的PID控制器

static PID_TypeDef pid_velocity;//速度环的PID的PID控制器

然后我们对其进行初始化

//
// @简介： 负责初始化平衡车控制系统
//
void App_Control_Init(void)
{
PID_Init(&pid_velocity,10.0f,1.0f,0.0f);
PID_LimitConfig(&pid_velocity,+0.5*g,-0.5*g);
PID_Init(&pid_theta,4.0f,0.0f,0.0f);//初始化θ环PID
PID_LimitConfig(&pid_theta,+12.57f,-12.57f);//将θ环的PID控制器输出限制在+-4PI rad/s之间
PID_Init(&pid_theta_dot,10.0f,10.0f,0.0f);//初始化θ点环PID
PID_LimitConfig(&pid_theta,+125.7f,-125.7f);//将θ点环的PID控制器输出限制在+-40PI rad/s^2之间
}

然后我们设置初始目标值为0，并获取轮胎的转速值

//
// @简介： 平衡车控制系统的进程函数
//
void App_Control_Proc(void)
{
PERIODIC(5) //控制程序每5ms执行一次
// #-1. 改变速度环的设定值
PID_ChangeSP(&pid_velocity,0.0f);
// #-2. 读取传感器的值
float omega = 0.5 * (App_Encoder_GetSpeed_L() + App_Encoder_GetSpeed_R());
float theta = App_MPU6050_GetPitch()*0.0174533;//单位rad
float theta_dot  =App_MPU6050_GetGx()*0.0174533;//单位rad
// #-3. 计算速度环的反馈值x_dot
float omega2 = -theta_dot * (lp+rw)/rw;
float omega1 = omega - omega2;
float x_dot = omega1 *rw;
// #-4. 执行速度环的PID运算
float theta_ref = qatan(PID_Compute(&pid_velocity,x_dot)/g);
// #1. 将外环的设定值SP设置为0
PID_ChangeSP(&pid_theta,theta_ref);
// #3. 计算外环PID的输出
float theta_dot_ref = PID_Compute(&pid_theta,theta);
// #4. 改变内环的设定值SP
PID_ChangeSP(&pid_theta_dot,theta_dot_ref);
// #5. 计算内环PID的输出
float theta_dot_dot_ref = PID_Compute(&pid_theta_dot,theta_dot);
// #6. 倒立摆的逆解算
float x_dot_dot_ref = (g*qsin(theta) - theta_dot_dot_ref*lp)/qcos(theta);
// #7. 计算轮胎转速
omega_ref += 1.0f/rw * x_dot_dot_ref * 0.005;
// #8. 设置轮胎的转速
App_Motor_SetOmega_L(omega_ref);
App_Motor_SetOmega_R(omega_ref);
}

这个时候我们就设定完成了，但是我们这个时候会发现，运行程序的时候轮子是会空转的，为了解决这个问题，所以按下按钮之后，进行复位操作。

//
// @简介：程序复位函数
//
void App_Control_Reset(void)
{
omega_ref = 0.0f;
PID_Reset(&pid_theta);
PID_Reset(&pid_theta_dot);
PID_Reset(&pid_velocity);
}

//
// @简介： 按键回调函数
//
static void OnUserKey_Clicked(uint8_t clicks)
{
if(clicks == 1)
{
pwm_on^=1;
App_Motor_Cmd(pwm_on);
App_Control_Reset();
}
}