16 double MotorModel::getCurrent(
double torque,
double dq,
double ddq,
double Kt_p,
double Kt_n,
double Kf_p,
double Kf_n,
17 double Kv_p,
double Kv_n,
double Ka_p,
double Ka_n,
unsigned int poly) {
18 assert(Kt_p > 0.0 &&
"Kt_p should be > 0");
19 assert(Kt_n > 0.0 &&
"Kt_n should be > 0");
20 assert(Kf_p >= 0.0 &&
"Kf_p should be >= 0");
21 assert(Kf_n >= 0.0 &&
"Kf_n should be >= 0");
22 assert(Kv_p >= 0.0 &&
"Kv_p should be >= 0");
23 assert(Kv_n >= 0.0 &&
"Kv_n should be >= 0");
24 assert(Ka_p >= 0.0 &&
"Ka_p should be >= 0");
25 assert(Ka_n >= 0.0 &&
"Ka_n should be >= 0");
27 double signDq = this->
smoothSign(dq, 0.1, poly);
31 double Kt = 0.5 * (Kt_p * (1 + signDq) + Kt_n * (1 - signDq));
32 double Kv = 0.5 * (Kv_p * (1 + signDq) + Kv_n * (1 - signDq));
33 double Ka = 0.5 * (Ka_p * (1 + signDq) + Ka_n * (1 - signDq));
34 double Kf = 0.5 * (Kf_p * (1 + signDq) + Kf_n * (1 - signDq));
36 current = Kt * torque + Kv * dq + Ka * ddq + signDq * Kf;
41 double MotorModel::getTorque(
double current,
double dq,
double ddq,
double Kt_p,
double Kt_n,
double Kf_p,
double Kf_n,
42 double Kv_p,
double Kv_n,
double Ka_p,
double Ka_n,
unsigned int poly) {
43 assert(Kt_p > 0.0 &&
"Kt_p should be > 0");
44 assert(Kt_n > 0.0 &&
"Kt_n should be > 0");
45 assert(Kf_p >= 0.0 &&
"Kf_p should be >= 0");
46 assert(Kf_n >= 0.0 &&
"Kf_n should be >= 0");
47 assert(Kv_p >= 0.0 &&
"Kv_p should be >= 0");
48 assert(Kv_n >= 0.0 &&
"Kv_n should be >= 0");
49 assert(Ka_p >= 0.0 &&
"Ka_p should be >= 0");
50 assert(Ka_n >= 0.0 &&
"Ka_n should be >= 0");
52 double signDq = this->
smoothSign(dq, 0.1, poly);
56 double Kt = 0.5 * (Kt_p * (1 + signDq) + Kt_n * (1 - signDq));
57 double Kv = 0.5 * (Kv_p * (1 + signDq) + Kv_n * (1 - signDq));
58 double Ka = 0.5 * (Ka_p * (1 + signDq) + Ka_n * (1 - signDq));
59 double Kf = 0.5 * (Kf_p * (1 + signDq) + Kf_n * (1 - signDq));
61 torque = (current / Kt) - (Kv / Kt) * dq - (Ka / Kt) * ddq - signDq * (Kf / Kt);
67 if (value > threshold)
69 else if (value < -threshold)
71 double a = value / threshold;
72 if (poly == 1)
return a;
73 if (poly == 2 && value > 0)
return a * a;
74 if (poly == 2 && value <= 0)
return -a * a;
double smoothSign(double value, double threshold, unsigned int poly=3)
double getTorque(double current, double dq, double ddq, double Kt_p, double Kt_n, double Kf_p=0.0, double Kf_n=0.0, double Kv_p=0.0, double Kv_n=0.0, double Ka_p=0.0, double Ka_n=0.0, unsigned int poly=3)
double getCurrent(double torque, double dq, double ddq, double Kt_p, double Kt_n, double Kf_p=0.0, double Kf_n=0.0, double Kv_p=0.0, double Kv_n=0.0, double Ka_p=0.0, double Ka_n=0.0, unsigned int poly=3)