working-est copy with class-based code. still open loop
Fork of analoghalls6 by
Diff: loopdriver.cpp
- Revision:
- 9:d3b70c15baa9
- Parent:
- 6:99ee0ce47fb2
- Child:
- 10:b4abecccec7a
--- a/loopdriver.cpp Wed Mar 04 15:33:32 2015 +0000 +++ b/loopdriver.cpp Fri Mar 06 19:12:53 2015 +0000 @@ -20,7 +20,7 @@ _user = user; _pid_d = pid_d; _pid_q = pid_q; - _reference = new SynchronousReferenceSynthesizer(max_phase_current);; + _reference = new SynchronousReferenceSynthesizer(max_phase_current); _modulator = modulator; _max_phase_current = max_phase_current; _update_frequency = update_frequency; @@ -31,26 +31,43 @@ } void LoopDriver::Clarke(float c, float b, float *alpha, float *beta) { - *alpha = b; - //*beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b; - *beta = (b + 2.0f * c)/sqrt(3.0f); + /* + //ignoring current for now. These are motor position vectors (A,B,C) but no C because it's not necessary for the calculation) + //these numbers range from -1:1, but when we use current instead of angle, the units will be amps. The amps will be unbounded ;) + float A, B; + A = FastSin(_motor->angle); + B = FastSin(_motor->angle - 120.0f); + + //\alpha \beta (Clarke) transform + *alpha = A; + *beta = (A + 2.0f*B)/sqrt(3.0f); + */ + //\alpha \beta (Clarke) transform + float ia = -(b+c); + float ib = b; + float ic = c; + + *alpha = ia; + *beta = (ia + 2.0f*ib)/sqrt(3.0f); } void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) { - float cos = LutCos(theta); - float sin = LutSin(theta); - *d = alpha * cos - beta * sin; - *q = -alpha * sin - beta * cos; + float cos = FastCos(theta); + float sin = FastSin(theta); + *d = alpha * cos + beta * sin; + *q = -alpha * sin + beta * cos; } void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) { - float cos = LutCos(theta); - float sin = LutSin(theta); - //*alpha = cos * d - sin * q; - //*beta = -(sin * d + cos * q); + float cos = FastCos(theta); + float sin = FastSin(theta); + *alpha = cos * d - sin * q; + *beta = sin * d + cos * q; + /* *alpha = cos * 1.0f; - *beta = -sin * 1.0f; + *beta = -sin * 1.0f; + */ } float LoopDriver::LutSin(float theta) { @@ -64,14 +81,36 @@ } void LoopDriver::update() { + float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta; Clarke(_motor->I_c, _motor->I_b, &alpha, &beta); - test_alpha = alpha; - test_beta = beta; Parke(alpha, beta, _motor->angle, &d, &q); - _reference->GetReference(_motor->angle, &ref_d, &ref_q); - float vd = _pid_d->Update(ref_d, d); - float vq = _pid_q->Update(ref_q, q); + + d_mean = 0.01f*d + 0.99f*d_mean; + q_mean = 0.01f*q + 0.99f*q_mean; + + _reference->GetReference(_motor->angle, &ref_d, &ref_q); //this just returns d = 0 and q = 1 right now. doesn't care about angle + float vd = _pid_d->Update(ref_d, d_mean); + float vq = _pid_q->Update(ref_q, q_mean); + + test_alpha = vd; + test_beta = vq; + + /* + //overriding PI controller here because we're not actually doing current control yet + //These values are approximately what I saw coming out of the park transform. just feeding them back in here to test the loop + vd = 0.0f; + vq = -1.0f; + */ + InverseParke(vd, vq, _motor->angle, &valpha, &vbeta); - _modulator->Update(valpha, vbeta); + + _modulator->Update(valpha, vbeta); + + /* + _inverter->SetDtcA((LutSin(_motor->angle)/2.0f)+0.5f); + _inverter->SetDtcB((LutSin(_motor->angle - 120.0f)/2.0f)+0.5f); + _inverter->SetDtcC((LutSin(_motor->angle + 120.0f)/2.0f)+0.5f); + */ + }