-
Notifications
You must be signed in to change notification settings - Fork 0
/
PD_controller.c
183 lines (155 loc) · 5.72 KB
/
PD_controller.c
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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
/*
* @Description: Impedance Controller
* @Author: Yangjian
* @Date: 2019-02-17 10:13:36
* @LastEditTime: 2019-08-28 15:24:28
* @LastEditors: Please set LastEditors
*/
#include "Impedance_Control.h"
#include "cmsis_os2.h" // ARM::CMSIS:RTOS2:Keil RTX5
#include <stdio.h>
#include "..\interface\interface.h"
#include "math.h"
/*需要调参*/
#define KP 450.0f // PD controller parameters
#define KD 35.0f // PD controller parameters
#define MASS_KNEE 2.811f //kg
#define COM_KNEE_Y 0.02982 //m
#define MOI_KNEE 0.01348f //kg*m^2
#define GRAV_VAR 9.794f // 重力加速度
#define MD 1
#define K 635 // 机械腿弹簧系数
#define B 0.000317f // motor parameter
#define PI 3.14159f
#define DELTA_T 0.01f//s
#define ALFA 0.2f // filter parameter
#define GAMA 15.0f
#define LAMBDA 19.2f // Kd = GAMA * LAMBDA
#define KZ 35.0f
#define KS 52.0f // parameters of terms of position control
#define K_THETA 0.15f // parameters of terms of velocity control
#define A_THETA 17.02f
static float T;
static float prev_q;
static float prev_q_dot;
static float prev_tau_l;
static float prev_qr_dot;
static float prev_qr_dot_dot;
static float prev_theta;
static float prev_theta_dot;
static float prev_theta_d;
static float prev_theta_d_dot;
static float prev_tau_l_dot;
static float prev_qd;
static int printf_cnt = 0;
float PD_main(knee_enc_abs_SEA_output, knee_enc_abs_SEA_input)
{
// 变量声明
float q;
float theta;
float qd;
float qd_dot;
float q_dot;
float delta_q_dot;
float delta_q;
float tau_sea;
float tau_l;
float z; // impedance vector
float qr_dot; // reference vector
float qr_dot_dot;
float theta_d;
float delta_theta;
float s; // sliding vector
float theta_r_dot;
float theta_dot;
float theta_d_dot;
float tau;
float G;
float inertia;
float moi;
float current;
float pwm;
float q_err;
float theta_err;
float qd_dot_dot;
// encoder2angle，编码值零点位置需要注意不要发生漂移(！以q的值为标准，q对零后再修改相对的theta值！)
theta = 0 - (float)((knee_enc_abs_SEA_output - 40468)) / 567.978;
q = 0 - (float)((knee_enc_abs_SEA_input - 25068)) / 567.978;
// 选择弧度或者角度进行计算
q = q * PI / 180;
theta = theta * PI / 180;
// 定义期望轨迹或者期望点
// qd = -0.70;
// qd_dot = 0;
qd = -0.2 + 0.2 * sin(PI * T );
qd_dot = 0.2 * PI * cos(PI * T);
// qd_dot = (qd - prev_qd) / DELTA_T;
// qd_dot_dot = -0.35 * PI * sin(PI * T);
// q_dot = ALFA * (q - prev_q) / DELTA_T + (1-ALFA) * prev_q_dot;
q_dot = (q - prev_q) / DELTA_T;
q_dot = ALFA * q_dot + (1-ALFA) * prev_q_dot;
delta_q = q - qd;
delta_q_dot = q_dot - qd_dot;
tau_sea = K * (theta - q); // 弹簧输出力矩
tau_l = (tau_sea * DELTA_T / MD + prev_tau_l) / (1 + DELTA_T * GAMA); //交互力低通滤波
// tau_l = (tau_sea * DELTA_T / (MD * ALFA) + prev_tau_l + (1-ALFA) * prev_tau_l_dot * DELTA_T) / (1+ DELTA_T * MATRIX_1);
// 重力项
G = - ( MASS_KNEE * GRAV_VAR * COM_KNEE_Y * sin(q) );
inertia = MASS_KNEE * COM_KNEE_Y + MOI_KNEE;
#if(0)
/*------------------------------------PD Controller--------------------------------------------*/
// tau = KP * (q_desired - q) + KD * q_dot ;// u = - Kp*(qd-q) - Kd*q_dot
tau = qd_dot_dot + KD * (qd_dot - q_dot) + KP * (qd - q);// u = qd_dot_dot + Kd * e_dot + Kp * e
current = tau / 4.628; //ratio
pwm = (float)(500 + current * 400 / 14 ); //百分之10 ~ 百分之90 ---> -14A ~ 14A
#else
/*---------------------------------IMPEDANCE Controller-------------------------------------------*/
// impedance vector z
qr_dot = qd_dot - LAMBDA * delta_q + tau_l;
// qr_dot_dot = ALFA * (qr_dot - prev_qr_dot) / DELTA_T + (1-ALFA) * prev_qr_dot_dot;
qr_dot_dot = (qr_dot - prev_qr_dot) / DELTA_T;
z = q_dot - qr_dot;
moi = inertia * qr_dot_dot;
// desired input
theta_d = q + (-KZ * z - tau_sea) / K;
// theta_d = q + (-KZ * z - tau_sea + G + moi ) / K;
delta_theta = theta - theta_d;
// theta_dot = ALFA * (theta - prev_theta) / DELTA_T + (1-ALFA) * prev_theta_dot;
// theta_d_dot = ALFA * (theta_d - prev_theta_d) / DELTA_T + (1-ALFA) * prev_theta_d_dot;
theta_dot = (theta - prev_theta) / DELTA_T ;
theta_d_dot = (theta_d - prev_theta_d) / DELTA_T;
// sliding vector
theta_r_dot = theta_d_dot - A_THETA * delta_theta;
s = theta_dot - theta_r_dot;
tau = tau_sea - KS * s - K_THETA * delta_theta;
current = tau / 4.628; //ratio
pwm = (float)(500 + current * 400 / 14 ); //百分之10 ~ 百分之90 ---> -14A ~ 14A
#endif
// error
q_err = q - qd;
theta_err = theta - theta_d;
// 串口打印
printf_cnt++;
if(printf_cnt > 0)
{
printf_cnt = 0;
// mprintf("z: %f, theta_err: %f ,theta: %f ,theta_d: %f, q_err: %f, q: %f ,qd: %f, %d %d\n", z, theta_err*180/PI, theta*180/PI, theta_d*180/PI, q_err*180/PI, q*180/PI, qd*180/PI, knee_enc_abs_SEA_output, knee_enc_abs_SEA_input);
mprintf("%f %f %f %f %f %f %f\n", z, theta_err*180/PI, theta*180/PI, theta_d*180/PI, q_err*180/PI, q*180/PI, qd*180/PI);
// mprintf("%f %f %f %f\n", delta_q_dot, LAMBDA * delta_q , tau_l, delta_q );
// mprintf("%f %f %f \n", theta_err, q_err, z );
}
/***************update*****************/
T = T + DELTA_T;
prev_q = q;
prev_q_dot = q_dot;
prev_tau_l = tau_l;
prev_qr_dot = qr_dot;
prev_qr_dot_dot = qr_dot_dot;
prev_theta = theta;
prev_theta_dot = theta_dot;
prev_theta_d = theta_d;
prev_theta_d_dot = theta_d_dot;
prev_tau_l_dot = ALFA * (tau_l - prev_tau_l) / DELTA_T + (1-ALFA) * prev_tau_l_dot;
prev_qd = qd;
return pwm;
}