N K
/
analoghalls6
motor spins
Fork of analoghalls5 by
loopdriver.cpp@6:99ee0ce47fb2, 2015-03-04 (annotated)
- Committer:
- nki
- Date:
- Wed Mar 04 15:33:32 2015 +0000
- Revision:
- 6:99ee0ce47fb2
- Parent:
- 5:ee1e6c84c302
3/4;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 1:1f58bdcf2956 | 1 | #include "includes.h" |
bwang | 1:1f58bdcf2956 | 2 | #include "core.h" |
bwang | 1:1f58bdcf2956 | 3 | #include "sensors.h" |
bwang | 1:1f58bdcf2956 | 4 | #include "meta.h" |
bwang | 3:0a2396597e0d | 5 | #include "lut.h" |
bwang | 1:1f58bdcf2956 | 6 | |
bwang | 3:0a2396597e0d | 7 | Inverter* LoopDriver::_inverter; |
bwang | 3:0a2396597e0d | 8 | Motor* LoopDriver::_motor; |
bwang | 3:0a2396597e0d | 9 | User* LoopDriver::_user; |
bwang | 3:0a2396597e0d | 10 | PidController* LoopDriver::_pid_d; |
bwang | 3:0a2396597e0d | 11 | PidController* LoopDriver::_pid_q; |
bwang | 3:0a2396597e0d | 12 | ReferenceSynthesizer* LoopDriver::_reference; |
bwang | 3:0a2396597e0d | 13 | Modulator* LoopDriver::_modulator; |
nki | 4:fdadf4a3577a | 14 | int LoopDriver::_blinky_toggle; |
bwang | 3:0a2396597e0d | 15 | |
bwang | 3:0a2396597e0d | 16 | LoopDriver::LoopDriver(Inverter *inverter, Motor *motor, User *user, PidController *pid_d, |
bwang | 3:0a2396597e0d | 17 | PidController *pid_q, Modulator *modulator, float max_phase_current, int update_frequency) { |
bwang | 3:0a2396597e0d | 18 | _inverter = inverter; |
bwang | 3:0a2396597e0d | 19 | _motor = motor; |
bwang | 3:0a2396597e0d | 20 | _user = user; |
bwang | 3:0a2396597e0d | 21 | _pid_d = pid_d; |
bwang | 3:0a2396597e0d | 22 | _pid_q = pid_q; |
bwang | 3:0a2396597e0d | 23 | _reference = new SynchronousReferenceSynthesizer(max_phase_current);; |
bwang | 3:0a2396597e0d | 24 | _modulator = modulator; |
bwang | 3:0a2396597e0d | 25 | _max_phase_current = max_phase_current; |
bwang | 3:0a2396597e0d | 26 | _update_frequency = update_frequency; |
bwang | 3:0a2396597e0d | 27 | } |
bwang | 3:0a2396597e0d | 28 | |
bwang | 3:0a2396597e0d | 29 | void LoopDriver::Start() { |
bwang | 3:0a2396597e0d | 30 | _upd_ticker.attach_us(&update, 1000000 / _update_frequency); |
bwang | 3:0a2396597e0d | 31 | } |
bwang | 3:0a2396597e0d | 32 | |
nki | 6:99ee0ce47fb2 | 33 | void LoopDriver::Clarke(float c, float b, float *alpha, float *beta) { |
nki | 6:99ee0ce47fb2 | 34 | *alpha = b; |
nki | 6:99ee0ce47fb2 | 35 | //*beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b; |
nki | 6:99ee0ce47fb2 | 36 | *beta = (b + 2.0f * c)/sqrt(3.0f); |
bwang | 3:0a2396597e0d | 37 | } |
bwang | 3:0a2396597e0d | 38 | |
bwang | 3:0a2396597e0d | 39 | void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) { |
bwang | 3:0a2396597e0d | 40 | float cos = LutCos(theta); |
bwang | 3:0a2396597e0d | 41 | float sin = LutSin(theta); |
nki | 6:99ee0ce47fb2 | 42 | *d = alpha * cos - beta * sin; |
nki | 6:99ee0ce47fb2 | 43 | *q = -alpha * sin - beta * cos; |
bwang | 3:0a2396597e0d | 44 | } |
bwang | 3:0a2396597e0d | 45 | |
bwang | 3:0a2396597e0d | 46 | void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) { |
bwang | 3:0a2396597e0d | 47 | float cos = LutCos(theta); |
bwang | 3:0a2396597e0d | 48 | float sin = LutSin(theta); |
nki | 6:99ee0ce47fb2 | 49 | //*alpha = cos * d - sin * q; |
nki | 6:99ee0ce47fb2 | 50 | //*beta = -(sin * d + cos * q); |
nki | 6:99ee0ce47fb2 | 51 | |
nki | 6:99ee0ce47fb2 | 52 | *alpha = cos * 1.0f; |
nki | 6:99ee0ce47fb2 | 53 | *beta = -sin * 1.0f; |
bwang | 3:0a2396597e0d | 54 | } |
bwang | 3:0a2396597e0d | 55 | |
bwang | 3:0a2396597e0d | 56 | float LoopDriver::LutSin(float theta) { |
bwang | 3:0a2396597e0d | 57 | if (theta < 0.0f) theta += 360.0f; |
bwang | 3:0a2396597e0d | 58 | if (theta >= 360.0f) theta -= 360.0f; |
nki | 6:99ee0ce47fb2 | 59 | return sinetab[(int) theta] * 2.0f - 1.0f; |
bwang | 3:0a2396597e0d | 60 | } |
bwang | 3:0a2396597e0d | 61 | |
bwang | 3:0a2396597e0d | 62 | float LoopDriver::LutCos(float theta) { |
bwang | 3:0a2396597e0d | 63 | return LutSin(90.0f - theta); |
bwang | 3:0a2396597e0d | 64 | } |
bwang | 3:0a2396597e0d | 65 | |
nki | 6:99ee0ce47fb2 | 66 | void LoopDriver::update() { |
bwang | 3:0a2396597e0d | 67 | float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta; |
nki | 6:99ee0ce47fb2 | 68 | Clarke(_motor->I_c, _motor->I_b, &alpha, &beta); |
nki | 6:99ee0ce47fb2 | 69 | test_alpha = alpha; |
nki | 6:99ee0ce47fb2 | 70 | test_beta = beta; |
bwang | 3:0a2396597e0d | 71 | Parke(alpha, beta, _motor->angle, &d, &q); |
bwang | 3:0a2396597e0d | 72 | _reference->GetReference(_motor->angle, &ref_d, &ref_q); |
bwang | 3:0a2396597e0d | 73 | float vd = _pid_d->Update(ref_d, d); |
bwang | 3:0a2396597e0d | 74 | float vq = _pid_q->Update(ref_q, q); |
bwang | 3:0a2396597e0d | 75 | InverseParke(vd, vq, _motor->angle, &valpha, &vbeta); |
nki | 6:99ee0ce47fb2 | 76 | _modulator->Update(valpha, vbeta); |
bwang | 3:0a2396597e0d | 77 | } |