速度环PID控制直流电机_码盘电机闭环控制

STM32电机PID速度控制

编码器
1.概述
编码器（encoder）是将信号（如比特流）或数据进行编制、转换为可用以通讯、传输和存储的信号形式的设备。编码器把角位移或直线位移转换成电信号，前者称为码盘，后者称为码尺。按照读出方式编码器可以分为接触式和非接触式两种；按照工作原理编码器可分为增量式和绝对式两类。
传感器-> 角速度或角位移------转化为------->电数字脉冲
2.原理
光电编码器是一种通过光电转换将输出轴上的机械几何位移量转换成脉冲或数字量的传感器。光电编码器是由光码盘和光电检测装置组成。光码盘是在一定直径的圆板上等分地开通若干个长方形孔。由于光电码盘与电动机同轴，电动机旋转时，检测装置检测输出若干脉冲信号，为判断转向，一般输出两组存在一定相位差的方波信号。

四倍频技术

PID理论

闭环控制是根据控制对象输出反馈来进行校正的控制方式，它是在测量出实际与计划发生偏差时，按定额或标准来进行纠正的。比如控制一个电机的转速，就得有一个测量转速的传感器，并将结果反馈到控制路线上。提到闭环控制算法，不得不提PID，它是闭环控制算法中最简单的一种。PID是比例 (Proportion) 积分 ,(Integral) 微分 ,(Differential coefficient) 的缩写，分别代表了三种控制算法。通过这三个算法的组合可有效地纠正被控制对象的偏差，从而使其达到一个稳定的状态。

PID增量式算法

离散化公式：
△u(k)= u(k)- u(k-1)
△u(k)=Kp[e(k)-e(k-1)]+Kie(k)+Kd[e(k)-2e(k-1)+e(k-2)]
进一步可以改写成：
△u(k)=Ae(k)-Be(k-1)+Ce(k-2)

PID位置算法

离散公式：
u(k)=Kpe(k) +Ki +Kd*[e(k)-e(k-1)]

代码分享

void Car_Init(void)
{ 
	motor.limitPwm= 7200;       //最大的PWM输出值
  motor.targetSpeed =1000 ;   //target   目标速度
	IncrementalPID_paraReset(&motor.incrementalpid, 0.9f, 0.24f, 1.0f, 7200, 500);
//  IncrementalPID_paraReset(&motor.incrementalpid, 0.5f, 5.0f, 0.5f, 7200, 500);  p0.9   i0.24   0.30
  PositionPID_paraReset(&motor.positionpid,1.0f, 0.0f, 0.0f, 7200, 500);
}

/**-
  * @brief  选择车的方向
  * @param  direction  Go_straight  向前走
	* @param             Go_back      向后走
  * @retval None
  */
void Car_Direction(int direction)
	{
	
	//选择车轮的旋转的方向
  switch(direction){
		
		case Go_straight:
			//此参数根据你们输出通用电平的引脚而定
		GPIO_ResetBits(GPIOA,GPIO_Pin_6);
    GPIO_SetBits(GPIOA,GPIO_Pin_7);
//			GPIO_SetBits(GPIOB,GPIO_Pin_14);
		
   	break;

    case Go_back:		
			//此参数根据你们输出通用电平的引脚而定			
//	    GPIO_ResetBits(GPIOB,GPIO_Pin_14);		
		GPIO_SetBits(GPIOA,GPIO_Pin_6);
    GPIO_ResetBits(GPIOA,GPIO_Pin_7);
			
		break;
		
	}
}



/**
  * @brief  读取当前脉冲值
  * @param  u8 TIMX   TIM1   定时器1
	* @param            TIM2   定时器2
	* @param            TIM3   定时器3
	* @param            TIM4   定时器4
	* @param            TIM5   定时器5
	* @param            TIM8   定时器8
  * @retval 当前的脉冲值
  */
int Encoder_Read(int timx)  //Encoder  编码器
{
	
   int Encoder_TIM; 
   switch(timx)
	 {
	   case Encoder_TIM1:  Encoder_TIM= (short)TIM1 -> CNT;  TIM1 -> CNT=0;break;
	   case Encoder_TIM2:  Encoder_TIM= (short)TIM2 -> CNT;  TIM2 -> CNT=0;break;
		 case Encoder_TIM3:  Encoder_TIM= (short)TIM3 -> CNT;  TIM3 -> CNT=0;break;
		 case Encoder_TIM4:  Encoder_TIM= (short)TIM4 -> CNT;  TIM4 -> CNT=0;break;
		 case Encoder_TIM5:  Encoder_TIM= (short)TIM5 -> CNT;  TIM5 -> CNT=0;break;
		 case Encoder_TIM8:  Encoder_TIM= (short)TIM8 -> CNT;  TIM8 -> CNT=0;break;
		 
		 default: Encoder_TIM=0;
	 }
		return Encoder_TIM;
	 
}


/**
  * @brief  输出限幅
  * @param  pwm  当前Pwm的值
  * @retval 经过限幅处理后的pwm值
  */
int Pwm_Limit(int pwm)
{
    
	int limit_pwm;
	limit_pwm = (pwm > motor.limitPwm)? motor.limitPwm:pwm;
  limit_pwm = (pwm < -motor.limitPwm)? motor.limitPwm:pwm;
 
	return limit_pwm;
}


/**
  * @brief  设置速度
  * @param  speed  当前速度的的值
  * @retval None
  */
void Speed_Set(int speed)
{
	
	TIM1->CCR1 = abs(speed);

}

/**
  * @brief  电机姿态检测及调整
  * @param  None
  * @retval None
  */
void Car_Contral(void)
{
	
	motor.encoder = Encoder_Read(Encoder_TIM4); 
	
	//两种PID模式供你们选择	
//	#if Icremental_PID_Mode	         增量式PID
		motor.motorPwm = Incremental_PID(&motor.incrementalpid,motor.targetSpeed,motor.encoder);  
	
//	#elif	Position_PID_PID_Mode	     位置式离散PID公式 
//		motor.motorPwm = Position_PID(&motor.positionpid,motor.targetSpeed,motor.encoder); 
//	#endif	
	
	//printf("%d",motor.encoder);
	//输出限幅，避免输出值大于电机承载的极限
	 motor.motorPwm = Pwm_Limit(motor.motorPwm);
	
	//根据计算出的输出值，判断电机的正反转
	if(motor.motorPwm>0)
	{
	  Car_Direction(Go_straight);
	}
	else 
		Car_Direction(Go_back);
	
	//将处理后的值，输出给电机
	Speed_Set(motor.motorPwm);

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

PID代码

void abs_limit(float *a, float ABS_MAX)
{
    if(*a > ABS_MAX) 
        *a = ABS_MAX;
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
}

void IncrementalPID_paraReset(incrementalpid_t *pid_t, float kp, float ki, float kd, uint32_t MaxOutput, uint32_t IntegralLimit)
{
	pid_t->Target = 0;
	pid_t->err = 0;
	pid_t->Measured = 0;
	pid_t->err_last = 0;
	pid_t->err_beforeLast = 0;
	pid_t->Kp = kp;
	pid_t->Ki = ki;
	pid_t->Kd = kd;
	pid_t->MaxOutput = MaxOutput;
	pid_t->IntegralLimit = IntegralLimit;
	pid_t->pwm = 0;
}

//void IncrementalPID_setPara(incrementalpid_t *pid_t, float kp, float ki, float kd)
//{
//	pid_t->err = 0;
//	pid_t->err_last = 0;
//	pid_t->err_beforeLast = 0;
//	pid_t->Kp = kp;
//	pid_t->Ki = ki;
//	pid_t->Kd = kd;
//	pid_t->pwm = 0;
//}


float Incremental_PID(incrementalpid_t *pid_t, float target, float measured) 
{
	float p_out, i_out, d_out;
	pid_t->Target = target;
	pid_t->Measured = measured;
	pid_t->err = pid_t->Target - pid_t->Measured;
	
//	if(abs(pid_t->err)<0.1f)
//		pid_t->err = 0.0f;
		//return 0;
	
	p_out = pid_t->Kp*(pid_t->err - pid_t->err_last);
	i_out = pid_t->Ki*pid_t->err;
	d_out = pid_t->Kd*(pid_t->err - 2.0f*pid_t->err_last + pid_t->err_beforeLast);
	
	//积分限幅
	abs_limit(&i_out, pid_t->IntegralLimit);
	
	pid_t->pwm += (p_out + i_out + d_out);
	
	//输出限幅
	abs_limit(&pid_t->pwm, pid_t->MaxOutput);
	
	pid_t->err_beforeLast = pid_t->err_last;
	pid_t->err_last = pid_t->err;

	return pid_t->pwm;
}

void PositionPID_paraReset(positionpid_t *pid_t, float kp, float ki, float kd, uint32_t MaxOutput, uint32_t IntegralLimit)
{
	pid_t->Target = 0;
	pid_t->Measured = 0;
	pid_t->MaxOutput = MaxOutput;
	pid_t->IntegralLimit = IntegralLimit;
	pid_t->err = 0;
	pid_t->err_last = 0;
	pid_t->integral_err = 0;
	pid_t->Kp = kp;
	pid_t->Ki = ki;
	pid_t->Kd = kd;
	pid_t->pwm = 0;	
}

//void PositionPID_setPara(positionpid_t *pid_t, float kp, float ki, float kd)
//	{
//	pid_t->err = 0;
//	pid_t->err_last = 0;
//	pid_t->integral_err = 0;
//	pid_t->Kp = kp;
//	pid_t->Ki = ki;
//	pid_t->Kd = kd;
//	pid_t->pwm = 0;
//}


float Position_PID(positionpid_t *pid_t, float target, float measured)
{
	float p_out, i_out, d_out;
	pid_t->Target = (float)target;
	pid_t->Measured = (float)measured;
	pid_t->err = pid_t->Target - pid_t->Measured;
	
//	if(abs(pid_t->err)<0.2f)
//		pid_t->err = 0.0f;
		//return 0;
	
	pid_t->integral_err += pid_t->err;
	
	p_out = pid_t->Kp*pid_t->err;
	i_out = pid_t->Ki*pid_t->integral_err;
	d_out = pid_t->Kd*(pid_t->err - pid_t->err_last);
	Test_I=i_out;
	//积分限幅
	abs_limit(&i_out, pid_t->IntegralLimit);
	
	pid_t->pwm = (p_out + i_out + d_out);
	
	//输出限幅
	abs_limit(&pid_t->pwm, pid_t->MaxOutput);
	
	pid_t->err_last = pid_t->err;
	
    
	
	return pid_t->pwm;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124