mirror of
https://github.com/rene-dev/stmbl.git
synced 2026-09-26 10:13:59 +08:00
+ pmsm, + mot, + idq, + cur piff
This commit is contained in:
crinq
+45
-133
@@ -1,158 +1,70 @@
|
||||
COMP(cur);
|
||||
|
||||
HAL_PIN(id) = 0.0;
|
||||
HAL_PIN(iq) = 0.0;
|
||||
HAL_PIN(id_cmd) = 0.0;
|
||||
HAL_PIN(iq_cmd) = 0.0;
|
||||
|
||||
HAL_PIN(ia) = 0.0;
|
||||
HAL_PIN(ib) = 0.0;
|
||||
HAL_PIN(id_fb) = 0.0;
|
||||
HAL_PIN(iq_fb) = 0.0;
|
||||
|
||||
HAL_PIN(iu) = 0.0;
|
||||
HAL_PIN(iv) = 0.0;
|
||||
HAL_PIN(iw) = 0.0;
|
||||
HAL_PIN(ud) = 0.0;
|
||||
HAL_PIN(uq) = 0.0;
|
||||
|
||||
HAL_PIN(i_u) = 0.0;
|
||||
HAL_PIN(i_v) = 0.0;
|
||||
HAL_PIN(i_w) = 0.0;
|
||||
HAL_PIN(volt) = 0.0;
|
||||
|
||||
HAL_PIN(u) = 0.0;
|
||||
HAL_PIN(v) = 0.0;
|
||||
HAL_PIN(w) = 0.0;
|
||||
HAL_PIN(rd) = 0.0;
|
||||
HAL_PIN(rq) = 0.0;
|
||||
HAL_PIN(ld) = 0.0;
|
||||
HAL_PIN(lq) = 0.0;
|
||||
|
||||
HAL_PIN(magpos) = 0.0;
|
||||
HAL_PIN(ff) = 0.0;
|
||||
HAL_PIN(kp) = 0.0;
|
||||
HAL_PIN(ki) = 0.0;
|
||||
|
||||
HAL_PIN(r) = 1.5;
|
||||
HAL_PIN(l) = 0.0025;
|
||||
HAL_PIN(freq) = 5000.0;
|
||||
HAL_PIN(id_error) = 0.0;
|
||||
HAL_PIN(iq_error) = 0.0;
|
||||
|
||||
HAL_PIN(volt) = 50.0;
|
||||
HAL_PIN(cor_volt) = 0.0;
|
||||
HAL_PIN(induction) = 0.0;
|
||||
HAL_PIN(ind_p) = 0.0;
|
||||
|
||||
HAL_PIN(pwm_max) = 0.95;
|
||||
HAL_PIN(cur_max) = 10.0;
|
||||
|
||||
HAL_PIN(p) = 0.0;
|
||||
HAL_PIN(ff) = 1.0;
|
||||
HAL_PIN(lp) = 1.0;
|
||||
HAL_PIN(cor_p) = 1.0;
|
||||
|
||||
MEM(float iu_old) = 0.0;
|
||||
MEM(float iv_old) = 0.0;
|
||||
MEM(float iw_old) = 0.0;
|
||||
MEM(float id_error_sum) = 0.0;
|
||||
MEM(float iq_error_sum) = 0.0;
|
||||
|
||||
RT(
|
||||
float r_ = MAX(PIN(r) / 2.0, 0.1);
|
||||
float l_ = MAX(PIN(l) / 2.0, 0.001);
|
||||
float freq_ = MAX(PIN(freq), 1);
|
||||
float vlt = MAX(PIN(volt), 0.1);
|
||||
//float ib_ = PIN(i);// + PIN(induction) / r_;
|
||||
float ind = PIN(induction);
|
||||
float ind_p_ = PIN(ind_p);
|
||||
float rd = MAX(PIN(rd), 0.1);
|
||||
float rq = MAX(PIN(rq), 0.1);
|
||||
float ld = MAX(PIN(ld), 0.001);
|
||||
float lq = MAX(PIN(ld), 0.001);
|
||||
float volt = MAX(PIN(volt), 0.1);
|
||||
|
||||
float magpos_ = PIN(magpos);
|
||||
float ff = PIN(ff);
|
||||
float kpd = ld * PIN(kp) / period;
|
||||
float kpq = lq * PIN(kp) / period;
|
||||
float kid = rd * PIN(ki) / period;
|
||||
float kiq = rq * PIN(ki) / period;
|
||||
|
||||
float s = sinf(magpos_);
|
||||
float c = cosf(magpos_);
|
||||
float idc = PIN(id_cmd);
|
||||
float iqc = PIN(iq_cmd);
|
||||
|
||||
float s3 = sqrtf(3.0);
|
||||
float id = PIN(id_fb);
|
||||
float iq = PIN(id_fb);
|
||||
|
||||
float id_error = (idc - id);
|
||||
float iq_error = (iqc - iq);
|
||||
|
||||
float id_ = LIMIT(PIN(id), PIN(cur_max));
|
||||
float iq_ = LIMIT(PIN(iq), PIN(cur_max));
|
||||
|
||||
float ia_ = id_ * c - iq_ * s; // inverse park
|
||||
float ib_ = id_ * s + iq_ * c;
|
||||
|
||||
float iu_ = ia_; // inverse clarke
|
||||
float iv_ = - ia_ / 2.0 + ib_ / 2.0 * s3;
|
||||
float iw_ = - ia_ / 2.0 - ib_ / 2.0 * s3;
|
||||
|
||||
|
||||
float ud_ = 0.0;
|
||||
float uq_ = ind * ind_p_;
|
||||
|
||||
float ua_ = ud_ * c - uq_ * s; // inverse park
|
||||
float ub_ = ud_ * s + uq_ * c;
|
||||
|
||||
float uu_ = ua_; // inverse clarke
|
||||
float uv_ = - ua_ / 2.0 + ub_ / 2.0 * s3;
|
||||
float uw_ = - ua_ / 2.0 - ub_ / 2.0 * s3;
|
||||
|
||||
|
||||
float ff_ = PIN(ff);
|
||||
float p_ = l_ * freq_ * PIN(lp);
|
||||
|
||||
float pwm_max_ = PIN(pwm_max);
|
||||
float vmax = vlt * pwm_max_;
|
||||
|
||||
float _u = CLAMP(ff_ * r_ * iu_ + p_ * (iu_ - iu_old) + uu_, -vmax / 2.0, vmax / 2.0);
|
||||
float _v = CLAMP(ff_ * r_ * iv_ + p_ * (iv_ - iv_old) + uv_, -vmax / 2.0, vmax / 2.0);
|
||||
float _w = CLAMP(ff_ * r_ * iw_ + p_ * (iw_ - iw_old) + uw_, -vmax / 2.0, vmax / 2.0);
|
||||
|
||||
iu_old += (_u - iu_old * r_ - uu_) / l_ / freq_;
|
||||
iv_old += (_v - iv_old * r_ - uv_) / l_ / freq_;
|
||||
iw_old += (_w - iw_old * r_ - uw_) / l_ / freq_;
|
||||
|
||||
float dead_time = 0.00000029;
|
||||
float pwm_res = 1200.0;
|
||||
float pwm_freq = 20000.0;
|
||||
float pwm_time_step = 1.0 / pwm_freq / pwm_res;
|
||||
float cor_val = dead_time / pwm_time_step;
|
||||
float cor_vlt = cor_val / pwm_res * vlt * PIN(cor_p);
|
||||
|
||||
_u -= SIGN(iu_old) * cor_vlt / 2.0;
|
||||
_v -= SIGN(iv_old) * cor_vlt / 2.0;
|
||||
_w -= SIGN(iw_old) * cor_vlt / 2.0;
|
||||
|
||||
PIN(cor_volt) = cor_vlt;
|
||||
|
||||
/* if(ABS(_uv) > ABS(_vw)){
|
||||
if(ABS(_uv) > ABS(_wu)){ // _uv
|
||||
_u = (vlt - _uv) / 2.0;
|
||||
_v = (vlt + _uv) / 2.0;
|
||||
_w = _v + _vw;
|
||||
}
|
||||
else{ // _wu
|
||||
_u = (vlt + _wu) / 2.0;
|
||||
_w = (vlt - _wu) / 2.0;
|
||||
_v = _u + _uv;
|
||||
}
|
||||
if(kid > 0.0){
|
||||
id_error_sum += id_error;
|
||||
iq_error_sum += iq_error;
|
||||
}
|
||||
else{
|
||||
if(ABS(_vw) > ABS(_wu)){ // _vw
|
||||
_v = (vlt - _vw) / 2.0;
|
||||
_w = (vlt + _vw) / 2.0;
|
||||
_u = _w + _wu;
|
||||
}
|
||||
else{ // _wu
|
||||
_u = (vlt + _wu) / 2.0;
|
||||
_w = (vlt - _wu) / 2.0;
|
||||
_v = _u + _uv;
|
||||
}
|
||||
id_error_sum = 0.0;
|
||||
iq_error_sum = 0.0;
|
||||
}
|
||||
*/
|
||||
|
||||
_u += vlt / 2.0;
|
||||
_v += vlt / 2.0;
|
||||
_w += vlt / 2.0;
|
||||
float ud = CLAMP(ff * rd * idc + kpd * id_error + kid * id_error_sum, -volt / 2.0, volt / 2.0);
|
||||
float uq = CLAMP(ff * rq * iqc + kpq * iq_error + kiq * iq_error_sum, -volt / 2.0, volt / 2.0);
|
||||
|
||||
PIN(u) = _u;
|
||||
PIN(v) = _v;
|
||||
PIN(w) = _w;
|
||||
PIN(ud) = ud;
|
||||
PIN(uq) = uq;
|
||||
|
||||
PIN(ia) = ia_;
|
||||
PIN(ib) = ib_;
|
||||
|
||||
PIN(iu) = iu_;
|
||||
PIN(iv) = iv_;
|
||||
PIN(iw) = iw_;
|
||||
|
||||
PIN(i_u) = iu_old;
|
||||
PIN(i_v) = iv_old;
|
||||
PIN(i_w) = iw_old;
|
||||
|
||||
PIN(p) = p_;
|
||||
PIN(id_error) = id_error;
|
||||
PIN(iq_error) = iq_error;
|
||||
);
|
||||
|
||||
ENDCOMP;
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
COMP(idq);
|
||||
|
||||
HAL_PIN(ud) = 0.0;
|
||||
HAL_PIN(uq) = 0.0;
|
||||
|
||||
HAL_PIN(pos) = 0.0;
|
||||
HAL_PIN(polecount) = 1.0;
|
||||
|
||||
//HAL_PIN(ua) = 0.0;
|
||||
//HAL_PIN(ub) = 0.0;
|
||||
|
||||
HAL_PIN(u) = 0.0;
|
||||
HAL_PIN(v) = 0.0;
|
||||
HAL_PIN(w) = 0.0;
|
||||
|
||||
HAL_PIN(svm) = 0.0;
|
||||
|
||||
HAL_PIN(volt) = 0.0;
|
||||
|
||||
RT(
|
||||
float ud = PIN(ud);
|
||||
float uq = PIN(uq);
|
||||
|
||||
float p = (int)MAX(PIN(polecount), 1.0);
|
||||
float pos = PIN(pos) * p;
|
||||
|
||||
float volt = MAX(PIN(volt), 0.1);
|
||||
|
||||
float c = cosf(pos);
|
||||
float s = sinf(pos);
|
||||
float s3 = sqrtf(3.0);
|
||||
|
||||
float ua = ud * c - uq * s; // inverse park
|
||||
float ub = ud * s + uq * c;
|
||||
|
||||
float u = ua; // inverse clarke
|
||||
float v = - ua / 2.0 + ub / 2.0 * s3;
|
||||
float w = - ua / 2.0 - ub / 2.0 * s3;
|
||||
|
||||
|
||||
|
||||
float tmp = 0.0;
|
||||
switch((int)(PIN(svm))){
|
||||
case 1: // flat bottom
|
||||
tmp = MIN(MIN(u, v), w);
|
||||
|
||||
u -= tmp;
|
||||
v -= tmp;
|
||||
w -= tmp;
|
||||
break;
|
||||
|
||||
case 2: // normal svm
|
||||
break;
|
||||
|
||||
default: // centered
|
||||
u += volt / 2.0;
|
||||
v += volt / 2.0;
|
||||
w += volt / 2.0;
|
||||
}
|
||||
|
||||
|
||||
PIN(u) = u;
|
||||
PIN(v) = v;
|
||||
PIN(w) = w;
|
||||
);
|
||||
|
||||
ENDCOMP;
|
||||
@@ -0,0 +1,58 @@
|
||||
COMP(mot);
|
||||
|
||||
//in
|
||||
HAL_PIN(torque) = 0.0;
|
||||
|
||||
HAL_PIN(mr) = 0.1;
|
||||
HAL_PIN(md) = 0.01;
|
||||
HAL_PIN(ml) = 0.0;
|
||||
HAL_PIN(j) = 0.0;
|
||||
|
||||
HAL_PIN(real_pos) = 0.0;
|
||||
HAL_PIN(kp) = 0.0;
|
||||
HAL_PIN(ki) = 0.0;
|
||||
|
||||
//out
|
||||
HAL_PIN(acc) = 0.0;
|
||||
HAL_PIN(vel) = 0.0;
|
||||
HAL_PIN(pos) = 0.0;
|
||||
HAL_PIN(pos_error) = 0.0;
|
||||
HAL_PIN(jerk) = 0.0;
|
||||
|
||||
|
||||
|
||||
MEM(float pos) = 0.0;
|
||||
MEM(float vel) = 0.0;
|
||||
MEM(float old_torque) = 0.0;
|
||||
|
||||
|
||||
RT(
|
||||
float torque = PIN(torque);
|
||||
float pos_error = minus(PIN(real_pos), pos);
|
||||
torque += PIN(ml);
|
||||
torque -= PIN(md) * vel;
|
||||
if(ABS(vel) <= 0.001 && ABS(torque) <= PIN(mr)){
|
||||
torque = 0;
|
||||
}
|
||||
else if(ABS(vel) <= 0.001){
|
||||
torque -= PIN(mr) * SIGN(torque);
|
||||
}
|
||||
else{
|
||||
torque -= PIN(mr) * SIGN(vel);
|
||||
}
|
||||
|
||||
float j = MAX(PIN(j), 0.00001);
|
||||
float acc = torque / j + PIN(ki) * pos_error;
|
||||
vel += acc * period;
|
||||
pos += (vel + PIN(kp) * pos_error) * period;
|
||||
|
||||
PIN(jerk) = (torque - old_torque) / period;
|
||||
old_torque = torque;
|
||||
|
||||
PIN(pos) = pos;
|
||||
PIN(vel) = vel;
|
||||
PIN(acc) = acc;
|
||||
PIN(pos_error) = pos_error;
|
||||
);
|
||||
|
||||
ENDCOMP;
|
||||
@@ -0,0 +1,48 @@
|
||||
COMP(pmsm);
|
||||
|
||||
//in
|
||||
HAL_PIN(km) = 0.01;
|
||||
HAL_PIN(r) = 1.0;
|
||||
HAL_PIN(ld) = 0.001;
|
||||
HAL_PIN(lq) = 0.001;
|
||||
HAL_PIN(polecount) = 1.0;
|
||||
|
||||
HAL_PIN(vel) = 0.0;
|
||||
HAL_PIN(ud) = 0.0;
|
||||
HAL_PIN(uq) = 0.0;
|
||||
|
||||
//out
|
||||
HAL_PIN(id) = 0.0;
|
||||
HAL_PIN(iq) = 0.0;
|
||||
HAL_PIN(psi_d) = 0.0;
|
||||
HAL_PIN(psi_q) = 0.0;
|
||||
HAL_PIN(torque) = 0.0;
|
||||
|
||||
MEM(float id) = 0.0;
|
||||
MEM(float iq) = 0.0;
|
||||
|
||||
RT(
|
||||
float p = (int)MAX(PIN(polecount), 1.0);
|
||||
float vel_e = PIN(vel) * p;
|
||||
float ld = MAX(PIN(ld), 0.0001);
|
||||
float lq = MAX(PIN(lq), 0.0001);
|
||||
float ud = PIN(ud);
|
||||
float uq = PIN(uq);
|
||||
float psi_m = MAX(PIN(km), 0.01);
|
||||
float r = MAX(PIN(r), 0.01);
|
||||
|
||||
float psi_d = ld * id + psi_m;
|
||||
float psi_q = lq * iq;
|
||||
id += (ud - r * id - vel_e * psi_q) / ld * period;
|
||||
iq += (uq - r * iq - vel_e * psi_d) / lq * period;
|
||||
|
||||
float t = 3.0 / 2.0 * p * (psi_m * iq + (ld - lq) * id * iq);
|
||||
|
||||
PIN(id) = id;
|
||||
PIN(iq) = iq;
|
||||
PIN(psi_d) = psi_d;
|
||||
PIN(psi_q) = psi_q;
|
||||
PIN(torque) = t;
|
||||
);
|
||||
|
||||
ENDCOMP;
|
||||
Reference in New Issue
Block a user