working-est copy with class-based code. still open loop
Fork of analoghalls6 by
loopdriver.cpp@9:d3b70c15baa9, 2015-03-06 (annotated)
- Committer:
- nki
- Date:
- Fri Mar 06 19:12:53 2015 +0000
- Revision:
- 9:d3b70c15baa9
- Parent:
- 6:99ee0ce47fb2
- Child:
- 10:b4abecccec7a
loop is closed.;
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; |
nki | 9:d3b70c15baa9 | 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 | 9:d3b70c15baa9 | 34 | /* |
nki | 9:d3b70c15baa9 | 35 | //ignoring current for now. These are motor position vectors (A,B,C) but no C because it's not necessary for the calculation) |
nki | 9:d3b70c15baa9 | 36 | //these numbers range from -1:1, but when we use current instead of angle, the units will be amps. The amps will be unbounded ;) |
nki | 9:d3b70c15baa9 | 37 | float A, B; |
nki | 9:d3b70c15baa9 | 38 | A = FastSin(_motor->angle); |
nki | 9:d3b70c15baa9 | 39 | B = FastSin(_motor->angle - 120.0f); |
nki | 9:d3b70c15baa9 | 40 | |
nki | 9:d3b70c15baa9 | 41 | //\alpha \beta (Clarke) transform |
nki | 9:d3b70c15baa9 | 42 | *alpha = A; |
nki | 9:d3b70c15baa9 | 43 | *beta = (A + 2.0f*B)/sqrt(3.0f); |
nki | 9:d3b70c15baa9 | 44 | */ |
nki | 9:d3b70c15baa9 | 45 | //\alpha \beta (Clarke) transform |
nki | 9:d3b70c15baa9 | 46 | float ia = -(b+c); |
nki | 9:d3b70c15baa9 | 47 | float ib = b; |
nki | 9:d3b70c15baa9 | 48 | float ic = c; |
nki | 9:d3b70c15baa9 | 49 | |
nki | 9:d3b70c15baa9 | 50 | *alpha = ia; |
nki | 9:d3b70c15baa9 | 51 | *beta = (ia + 2.0f*ib)/sqrt(3.0f); |
bwang | 3:0a2396597e0d | 52 | } |
bwang | 3:0a2396597e0d | 53 | |
bwang | 3:0a2396597e0d | 54 | void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) { |
nki | 9:d3b70c15baa9 | 55 | float cos = FastCos(theta); |
nki | 9:d3b70c15baa9 | 56 | float sin = FastSin(theta); |
nki | 9:d3b70c15baa9 | 57 | *d = alpha * cos + beta * sin; |
nki | 9:d3b70c15baa9 | 58 | *q = -alpha * sin + beta * cos; |
bwang | 3:0a2396597e0d | 59 | } |
bwang | 3:0a2396597e0d | 60 | |
bwang | 3:0a2396597e0d | 61 | void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) { |
nki | 9:d3b70c15baa9 | 62 | float cos = FastCos(theta); |
nki | 9:d3b70c15baa9 | 63 | float sin = FastSin(theta); |
nki | 9:d3b70c15baa9 | 64 | *alpha = cos * d - sin * q; |
nki | 9:d3b70c15baa9 | 65 | *beta = sin * d + cos * q; |
nki | 6:99ee0ce47fb2 | 66 | |
nki | 9:d3b70c15baa9 | 67 | /* |
nki | 6:99ee0ce47fb2 | 68 | *alpha = cos * 1.0f; |
nki | 9:d3b70c15baa9 | 69 | *beta = -sin * 1.0f; |
nki | 9:d3b70c15baa9 | 70 | */ |
bwang | 3:0a2396597e0d | 71 | } |
bwang | 3:0a2396597e0d | 72 | |
bwang | 3:0a2396597e0d | 73 | float LoopDriver::LutSin(float theta) { |
bwang | 3:0a2396597e0d | 74 | if (theta < 0.0f) theta += 360.0f; |
bwang | 3:0a2396597e0d | 75 | if (theta >= 360.0f) theta -= 360.0f; |
nki | 6:99ee0ce47fb2 | 76 | return sinetab[(int) theta] * 2.0f - 1.0f; |
bwang | 3:0a2396597e0d | 77 | } |
bwang | 3:0a2396597e0d | 78 | |
bwang | 3:0a2396597e0d | 79 | float LoopDriver::LutCos(float theta) { |
bwang | 3:0a2396597e0d | 80 | return LutSin(90.0f - theta); |
bwang | 3:0a2396597e0d | 81 | } |
bwang | 3:0a2396597e0d | 82 | |
nki | 6:99ee0ce47fb2 | 83 | void LoopDriver::update() { |
nki | 9:d3b70c15baa9 | 84 | |
bwang | 3:0a2396597e0d | 85 | float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta; |
nki | 6:99ee0ce47fb2 | 86 | Clarke(_motor->I_c, _motor->I_b, &alpha, &beta); |
bwang | 3:0a2396597e0d | 87 | Parke(alpha, beta, _motor->angle, &d, &q); |
nki | 9:d3b70c15baa9 | 88 | |
nki | 9:d3b70c15baa9 | 89 | d_mean = 0.01f*d + 0.99f*d_mean; |
nki | 9:d3b70c15baa9 | 90 | q_mean = 0.01f*q + 0.99f*q_mean; |
nki | 9:d3b70c15baa9 | 91 | |
nki | 9:d3b70c15baa9 | 92 | _reference->GetReference(_motor->angle, &ref_d, &ref_q); //this just returns d = 0 and q = 1 right now. doesn't care about angle |
nki | 9:d3b70c15baa9 | 93 | float vd = _pid_d->Update(ref_d, d_mean); |
nki | 9:d3b70c15baa9 | 94 | float vq = _pid_q->Update(ref_q, q_mean); |
nki | 9:d3b70c15baa9 | 95 | |
nki | 9:d3b70c15baa9 | 96 | test_alpha = vd; |
nki | 9:d3b70c15baa9 | 97 | test_beta = vq; |
nki | 9:d3b70c15baa9 | 98 | |
nki | 9:d3b70c15baa9 | 99 | /* |
nki | 9:d3b70c15baa9 | 100 | //overriding PI controller here because we're not actually doing current control yet |
nki | 9:d3b70c15baa9 | 101 | //These values are approximately what I saw coming out of the park transform. just feeding them back in here to test the loop |
nki | 9:d3b70c15baa9 | 102 | vd = 0.0f; |
nki | 9:d3b70c15baa9 | 103 | vq = -1.0f; |
nki | 9:d3b70c15baa9 | 104 | */ |
nki | 9:d3b70c15baa9 | 105 | |
bwang | 3:0a2396597e0d | 106 | InverseParke(vd, vq, _motor->angle, &valpha, &vbeta); |
nki | 9:d3b70c15baa9 | 107 | |
nki | 9:d3b70c15baa9 | 108 | _modulator->Update(valpha, vbeta); |
nki | 9:d3b70c15baa9 | 109 | |
nki | 9:d3b70c15baa9 | 110 | /* |
nki | 9:d3b70c15baa9 | 111 | _inverter->SetDtcA((LutSin(_motor->angle)/2.0f)+0.5f); |
nki | 9:d3b70c15baa9 | 112 | _inverter->SetDtcB((LutSin(_motor->angle - 120.0f)/2.0f)+0.5f); |
nki | 9:d3b70c15baa9 | 113 | _inverter->SetDtcC((LutSin(_motor->angle + 120.0f)/2.0f)+0.5f); |
nki | 9:d3b70c15baa9 | 114 | */ |
nki | 9:d3b70c15baa9 | 115 | |
bwang | 3:0a2396597e0d | 116 | } |