From ee6404af8281501185049b9968e1a47ae065ebf4 Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 26 Jun 2015 10:28:26 +0200 Subject: [PATCH] + pmsm, + mot, + idq, + cur piff --- src/comps/cur.comp | 178 +++++++++++--------------------------------- src/comps/idq.comp | 67 +++++++++++++++++ src/comps/mot.comp | 58 +++++++++++++++ src/comps/pmsm.comp | 48 ++++++++++++ 4 files changed, 218 insertions(+), 133 deletions(-) create mode 100644 src/comps/idq.comp create mode 100644 src/comps/mot.comp create mode 100644 src/comps/pmsm.comp diff --git a/src/comps/cur.comp b/src/comps/cur.comp index a2f79fe5..ffd90ccb 100644 --- a/src/comps/cur.comp +++ b/src/comps/cur.comp @@ -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; diff --git a/src/comps/idq.comp b/src/comps/idq.comp new file mode 100644 index 00000000..2335b6c5 --- /dev/null +++ b/src/comps/idq.comp @@ -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; diff --git a/src/comps/mot.comp b/src/comps/mot.comp new file mode 100644 index 00000000..9a05d2dc --- /dev/null +++ b/src/comps/mot.comp @@ -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; diff --git a/src/comps/pmsm.comp b/src/comps/pmsm.comp new file mode 100644 index 00000000..29995ffb --- /dev/null +++ b/src/comps/pmsm.comp @@ -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;