增量式和位置式速度环（模块化）

       简述

        速度环是精确控制想要速度的一种方式，根据测量值反馈然后调节占空比的方法，原理大大致是这样，用简单的PID即可实现，这里我给出模块化的代码方便大家调用，大家可以先用上之后再细细了解它的原理。因为调函数是已经很舒服的事情，我将他模块化之后造福大家。下面是两种速度环，函数都已经给出。

        预处理

         在写速度环之前，你得先写一个函数可以控制左右占空比的，这样方便调用

比如写一个CarForward函数，参数就是左右的占空比。还有计算速度的函数，即直接用库函数读取这个时候编码器的脉冲值（注意这个不是单位m/s为单位的速度而且一定时间内的脉冲值当成速度而已）

        大部分单片机都有这种库，依据库来写很快的，我以stc32的单片机为例。因为我的库函数pwm_duty参数是uint16类型的不能是负数，所以我弄了另一个参数控制方向，之后才写的CarForward函数

void Motor_B_RUN(uint8 commond, uint32 MotorDuty1)//左
{ 
	if(MotorDuty1>=8000)
	{
	MotorDuty1=8000;
	}

	
	 if(commond==2)//commond =2 反转
	{
				pwm_duty(B_in_1,MotorDuty1);       //引脚
				pwm_duty(B_in_2,0);     		//引脚
	}
	else if(commond==1)//commond =1 正转转
	{
				pwm_duty(B_in_1,0);          //引脚
				pwm_duty(B_in_2, MotorDuty1);  	//引脚
	}
}

void Motor_A_RUN(uint8 commond, uint32 MotorDuty2)//右
{ 
	if(MotorDuty2>=8000)MotorDuty2=8000;
	 if(commond==2)//commond =2 反转
	{
			pwm_duty(A_in_1,0);          
            pwm_duty(A_in_2,MotorDuty2);			
	}
	else if(commond==1)//commond =1 正转
	{
			pwm_duty(A_in_1,MotorDuty2);          
            pwm_duty(A_in_2, 0);			
	}
}
/**
* @brief 占空比控制函数
* @param A_duty A控制的电机占空比，B_duty B控制的电机占空比
* @retval none
*/
void CarForward(int16 A_duty,int16 B_duty)//右 左
{
	if(A_duty>=0)Motor_A_RUN(1,A_duty);	
	else if(A_duty<=0)Motor_A_RUN(2,-A_duty);	
	if(B_duty>=0)Motor_B_RUN(1,B_duty);
	else if(B_duty<=0)Motor_B_RUN(2,-B_duty);
	
}

       直接读取脉冲值（即计算速度函数），下面要调用的

//编码器
#define SPEEDL_PLUSE   CTIM0_P34
#define SPEEDR_PLUSE   CTIM3_P04
//定义方向引脚
#define L_DIR     P35
#define R_DIR     P53
void  Velocity_Calu()
{
	
	VeL_actual =ctimer_count_read(SPEEDL_PLUSE); //右轮速度
	VeR_actual =ctimer_count_read(SPEEDR_PLUSE); //左轮速度	
	ctimer_count_clean(SPEEDL_PLUSE);//计数器清零
	ctimer_count_clean(SPEEDR_PLUSE);	
	if (L_DIR == 0)VeL_actual = -VeL_actual;
	if (R_DIR == 1)VeR_actual = -VeR_actual;		
}

  位置式模块化函数

float V_Kp_R,V_Ki_R ,V_Kp_L,V_Ki_L;//可以在这里直接填参数，也可以放在其他地方调

/**
* @brief 位置式速度环
/*优点：
①位置式PID是一种非递推式算法，可直接控制执行机构（如平衡小车），
u(k)的值和执行机构的实际位置（如小车当前角度）是一一对应的，
因此在执行机构不带积分部件的对象中可以很好应用
缺点：
①每次输出均与过去的状态有关，计算时要对e(k)进行累加，运算工作量大。
/*
* @param Goal_A目标左轮速度，Goal_B目标右轮速度
* @retval none
*/

int16 duty_l=0,duty_r=0;//vr,vl
float error_l=0,error_r=0;last_error_l=0,last_error_r=0;
void Sudu_Huan(float Goal_A, float Goal_B)// 位置式 (left,right);
{//修改
	static float integral_l = 0,integral_r = 0;
	Velocity_Calu();	
	error_l = Goal_A - VeL_actual; 
    error_r = Goal_B - VeR_actual;  
	integral_l += error_l;//积分位
	integral_r += error_r;  
     duty_l =(int16)( Goal_A+V_Kp_L * (error_l)+ V_Ki_L * integral_l);
     duty_r =(int16)( Goal_B+V_Kp_R * (error_r)+ V_Ki_R * integral_r);	
	if (integral_l > 5000)integral_l=5000;
	if (integral_r > 5000)integral_r=5000;
	if(duty_l > 9000)duty_l=9000;
	if(duty_r > 9000)duty_r=9000;
    // 将电机电压应用到电机上
    CarForward(duty_r,duty_l);
}

增量式模块化函数

float V_Kp_R,V_Ki_R ,V_Kp_L,V_Ki_L;//可以在这里直接填参数，也可以放在其他地方调
/**
* @brief 增量式速度环
/*
优点：
①误动作时影响小，必要时可用逻辑判断的方法去掉出错数据。
②手动/自动切换时冲击小，便于实现无扰动切换。当计算机故障时，仍能保持原值。
③算式中不需要累加。控制增量Δu(k)的确定仅与最近3次的关。
缺点：
①积分截断效应大，有稳态误差；
②溢出的影响大。有的被控对象用增量式则不太好；
/*
* @param Goal_A目标左轮速度，Goal_B目标右轮速度
* @retval none
*/
//之后占空比就没用了，只是用编码器的值非线性表示

int16 duty_l=0,duty_r=0;//vr,vl
float error_l=0,error_r=0;last_error_l=0,last_error_r=0;
void Sudu_Huan_2(float Goal_A, float Goal_B) //增量式 (left,right);
{
	float VeL_out_Kp,VeL_out_Ki,VeR_out_Kp,VeR_out_Ki;
	Velocity_Calu();
	error_l = Goal_A - VeL_actual; 
     VeL_out_Kp=V_Kp_L * (error_l - last_error_l);
	 VeL_out_Ki=V_Ki_L * error_l;
	last_error_l=error_l;	
	duty_l=duty_l+VeL_out_Kp + VeL_out_Ki;
	
	error_r = Goal_B - VeR_actual; 
     VeR_out_Kp=V_Kp_R * (error_r - last_error_r);
	 VeR_out_Ki=V_Ki_R * error_r;
	last_error_r=error_r;	
	duty_r=duty_r+VeR_out_Kp + VeR_out_Ki;
      if(duty_l<-9000)duty_l=-9000;       
      if(duty_l>9000) duty_l=9000;
	  if(duty_r<-9000)duty_r=-9000;       
      if(duty_r>9000) duty_r=9000;
    CarForward(duty_r,duty_l);
		
}

        用的时候直接填参数就行，但是记住这是后电机满转的是后用占空比我这个是10000，但是这个函数的参数200多差不多就满转了，大家可以直接几十几十的加，试出你想要的速度。

比如在主循环里写这个，参数一点一点加就行。

Sudu_Huan_2(110,110);//这个是增量式

        我用的是增量式就是上面这种，你们可以依据自己的需求调用，如果有选择困难症的话跟我用增量式就行，其实调用函数的时候我们只管那个最有效，什么缺点优点都是理论上的，实践才是检验真理的唯一标准，两个都可以试一下。

      觉得有用的话给我点个赞吧o(￣▽￣)ｄ