两轮平衡小车制作保姆式教程（3）——直立环、速度环、转向环

✅作者简介：大家好我是：麦克斯科技，希望一起努力，一起进步！

📃个人主页：麦克斯科技

🔥系列专栏：两轮平衡小车制作保姆式教程
🏷️非常欢迎大家在评论区留言交流，互相学习！

提前声明：博客中给出的代码经过多个项目测试，实测能用，性能稳定，请大家放心使用！

前言

本系列博客将从硬件到软件详细介绍“如何制作一辆两轮自平衡小车”，笔者毫无保留，以最通俗易懂的语言，以最简单的实现方案，分享自己从0到1制作平衡小车的全过程，相信跟着我的教程，大家也能顺利制作一台属于自己的平衡车。系列专栏：🔥两轮平衡小车制作保姆式教程🔥

首先，给大家提前交个底，其实制作一台平衡小车并不难，用到的主要模块就是陀螺仪，而最主要的控制算法就是PID算法，而且平衡小车对陀螺仪与PID算法的掌握程度要求并不是很高，所以适合初学者来作为项目练手。

该系列教程一共分为4个板块，分为《硬件选型》、《软件模块》、《直立环、速度环、转向环》、《调参保姆级教程》，4个板块条理清晰，层次分明，简明扼要，请大家跟着我开始学习吧！

直立环

作用：使小车保持直立。

//****************平衡小车机械零点***************
float Car_zero = 1.0f;    
//直立环 
float zhili_Kp=95.5f*0.6f;  //出现大幅度低频振荡 95.5f
float zhili_Kd=-10.0f*0.6f;    //出现小幅度高频振荡  *0.6f
int zhili_out=0;       //直立环输出

//***********平衡车控制******************************************
//函数功能：控制小车保持直立
//Angle：采集到的实际角度值  angle.x
//Gyro： 采集到的实际角速度值  gyroscope.x
int zhili(float Angle,float Gyro)
{  
   float err;
	 int pwm_zhili;
	 err=Car_zero-Angle;    //期望值-实际值，这里期望小车平衡，因此期望值就是机械中值       
	 pwm_zhili=zhili_Kp*err+Gyro*zhili_Kd;//计算平衡控制的电机PWM
	 return pwm_zhili;
}

速度环

作用：使小车平衡得更加稳定，平衡效果更好。

void speed_control_100hz(void)
{
	//平衡车直立环上的速度环，为与直立环处理速度一致，把下面两行代码注释掉
//	static uint16_t _cnt=0;
//	_cnt++;	if(_cnt<2)	return;	_cnt=0;//10ms控制一次 100hz是0.005s，两个100hz就是10ms

	speed_feedback[0]=smartcar_imu.left_motor_speed_cmps;//获取左轮实际值  
	speed_error[0]=v_target_l-speed_feedback[0];
	speed_error[0]=constrain_float(speed_error[0],-speed_err_max,speed_err_max);
	speed_integral[0]+=speed_ki*speed_error[0];
	speed_integral[0]=constrain_float(speed_integral[0],-speed_integral_max,speed_integral_max);
	speed_output[0]=speed_integral[0]+speed_kp*speed_error[0];
	speed_output[0]=constrain_float(speed_output[0],-speed_ctrl_output_max,speed_ctrl_output_max);
//	UART_printf(UART0_BASE,"%d\n",(int)smartcar_imu.left_motor_speed_cmps);
		
	speed_feedback[1]=smartcar_imu.right_motor_speed_cmps;//右轮
	speed_error[1]=v_target_r-speed_feedback[1];//期望速度减去实际速度得到速度误差
	speed_error[1]=constrain_float(speed_error[1],-speed_err_max,speed_err_max);//对速度误差做约束
	speed_integral[1]+=speed_ki*speed_error[1]; //速度积分
	speed_integral[1]=constrain_float(speed_integral[1],-speed_integral_max,speed_integral_max);//对得到的速度积分做约束
	speed_output[1]=speed_integral[1]+speed_kp*speed_error[1];//pid得到输出
	speed_output[1]=constrain_float(speed_output[1],-speed_ctrl_output_max,speed_ctrl_output_max);//对输出做约束
	//UART_printf(UART0_BASE,"%d\n",(int)smartcar_imu.right_motor_speed_cmps);
}

转向环

作用：优化小车的平衡效果，小车不会左拐和右拐，只在正前方与正后方调整车身来平衡车体。

//转向环
float zhuan_Kp=0.0f;   //期望小车转向，正反馈
float zhuan_Kd=-10.0f;    //抑制小车转向，负反馈

//*************************************************************
//函数功能：控制小车转向
//Set_turn：目标旋转角速度
//Gyro_Z:陀螺仪Z轴的角速度
//不是一个严格的PD控制器，为小车的叠加控制
int zhuan(float Set_turn,float Gyro_Z)
{
  int PWM_Out=0; 
	if(Set_turn==0)
	{
	 PWM_Out=zhuan_Kd*Gyro_Z; //没有转向需求，Kd约束小车转向
	}
	if(Set_turn!=0)
	{
	 PWM_Out=zhuan_Kp*Set_turn; //有转向需求，Kp为期望小车转向 
	}
	return PWM_Out;
}