平衡车 -- 速度环

🌈个人主页：羽晨同学

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

速度环

首先，我们创建一个速度环的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);//初始化θ环PIDPID_LimitConfig(&pid_theta,+12.57f,-12.57f);//将θ环的PID控制器输出限制在+-4PI rad/s之间PID_Init(&pid_theta_dot,10.0f,10.0f,0.0f);//初始化θ点环PIDPID_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;//单位radfloat theta_dot  =App_MPU6050_GetGx()*0.0174533;//单位rad// #-3. 计算速度环的反馈值x_dotfloat 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设置为0PID_ChangeSP(&pid_theta,theta_ref);// #3. 计算外环PID的输出float theta_dot_ref = PID_Compute(&pid_theta,theta);// #4. 改变内环的设定值SPPID_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();}
}