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
|
**//增量式PID**
void IncPidCalc(float SetKp, float SetKi, float SetKd,uint16 SetPoint, int16 GetEncoderSpeed)
{
static int16 SetPWM=0;
static int16 uk=0;
Kp=SetKp;
Ki=SetKi;
Kd=SetKd;
Error = SetPoint- GetEncoderSpeed;
uk=(uint16)(Kp*(Error-LastError)+Ki*Error+Kd*(Error-2*LastError+PrevError));
PrevError=LastError;
LastError=Error;
SetPWM+=uk;
if(SetPWM > 8000)SetPWM = 8000;
if(SetPWM <-8000)SetPWM =-8000;
if(SetPWM >= 0){
ftm_pwm_duty(FTM0, FTM_CH2, SetPWM );
ftm_pwm_duty(FTM0, FTM_CH3, SetPWM );
}
if(SetPWM < 0){
ftm_pwm_duty(FTM0, FTM_CH2, SetPWM );
ftm_pwm_duty(FTM0, FTM_CH3, SetPWM );
}
}
**//位置式PID**
void IncPidCalc2(float SetKp, float SetKi, float SetKd,uint16 SetPoint, int16 GetEncoderSpeed)
{
static int16 SetPWM=0;
static int16 uk=0,ErrorSum;
Kp=SetKp;
Ki=SetKi;
Kd=SetKd;
Error = SetPoint - GetEncoderSpeed;
ErrorSum += Error;
uk=(uint16)(Kp*Error+Ki*ErrorSum+Kd*(Error-LastError));
LastError=Error;
SetPWM=uk;
if(SetPWM > 7000)SetPWM = 7000;
if(SetPWM <-7000)SetPWM =-7000;
if(SetPWM >= 0){
ftm_pwm_duty(FTM0, FTM_CH2, SetPWM );
ftm_pwm_duty(FTM0, FTM_CH3, SetPWM );
}
if(SetPWM < 0){
ftm_pwm_duty(FTM0, FTM_CH2, SetPWM );
ftm_pwm_duty(FTM0, FTM_CH3, SetPWM );
}
}
**//改造后PID**
void IncPidCalc3(float SetKp, float SetKi, float SetKd,int16 SetPoint, int16 GetEncoderSpeed)
{
static int16 ErrorSum=0;
static int16 SetPWM=0,SetLastPWM=0;
SetPoint0=SetPoint;
Kp=SetKp;
Ki=SetKi;
Kd=SetKd;
Error=SetPoint-GetEncoderSpeed;
ErrorSum+=Error;
if(ErrorSum >= 13000) ErrorSum = 13000;
if(ErrorSum <= -4000) ErrorSum = -4000;
SetPWM=(int16)(SetLastPWM+Kp*Error+Ki*ErrorSum);
if(SetPWM>0){
if(SetPWM>8000)
{
SetPWM=8000;
}
else{
ftm_pwm_duty(FTM0,FTM_CH2,SetPWM);
ftm_pwm_duty(FTM0,FTM_CH3,0);
}
}
else{
SetPWM=-SetPWM;
if(SetPWM>2000){
SetPWM=2000;
}
else{
ftm_pwm_duty(FTM0,FTM_CH2,0);
ftm_pwm_duty(FTM0,FTM_CH3,SetPWM);
}
}
SetLastPWM=(int)SetPWM;
}
|
近期评论