N K / Mbed 2 deprecated analoghalls5_5

working-est copy with class-based code. still open loop

Dependencies:   mbed

Fork of analoghalls6 by N K

loopdriver.cpp@5:ee1e6c84c302, 2015-03-03 (annotated)

Committer:
nki
Date:
Tue Mar 03 06:28:10 2015 +0000
Revision:
5:ee1e6c84c302
Parent:
4:fdadf4a3577a
Child:
6:99ee0ce47fb2
hi there;

Who changed what in which revision?

UserRevisionLine numberNew 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
bwang 3:0a2396597e0d 33 void LoopDriver::Clarke(float a, float b, float *alpha, float *beta) {
bwang 3:0a2396597e0d 34 *alpha = a;
bwang 3:0a2396597e0d 35 *beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b;
bwang 3:0a2396597e0d 36 }
bwang 3:0a2396597e0d 37
bwang 3:0a2396597e0d 38 void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) {
bwang 3:0a2396597e0d 39 float cos = LutCos(theta);
bwang 3:0a2396597e0d 40 float sin = LutSin(theta);
bwang 3:0a2396597e0d 41 *d = alpha * cos + beta * sin;
bwang 3:0a2396597e0d 42 *q = -alpha * sin + beta * cos;
bwang 3:0a2396597e0d 43 }
bwang 3:0a2396597e0d 44
bwang 3:0a2396597e0d 45 void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) {
bwang 3:0a2396597e0d 46 float cos = LutCos(theta);
bwang 3:0a2396597e0d 47 float sin = LutSin(theta);
bwang 3:0a2396597e0d 48 *alpha = cos * d - sin * q;
bwang 3:0a2396597e0d 49 *beta = sin * d + cos * q;
bwang 3:0a2396597e0d 50 }
bwang 3:0a2396597e0d 51
bwang 3:0a2396597e0d 52 float LoopDriver::LutSin(float theta) {
bwang 3:0a2396597e0d 53 if (theta < 0.0f) theta += 360.0f;
bwang 3:0a2396597e0d 54 if (theta >= 360.0f) theta -= 360.0f;
nki 5:ee1e6c84c302 55 return (sinetab[(int) theta]*2)-1.0f; //shift range 0:1 to -1:1
bwang 3:0a2396597e0d 56 }
bwang 3:0a2396597e0d 57
bwang 3:0a2396597e0d 58 float LoopDriver::LutCos(float theta) {
bwang 3:0a2396597e0d 59 return LutSin(90.0f - theta);
bwang 3:0a2396597e0d 60 }
bwang 3:0a2396597e0d 61
bwang 3:0a2396597e0d 62 void LoopDriver::update() {
nki 5:ee1e6c84c302 63 /*
nki 4:fdadf4a3577a 64 _inverter->SetDtcA(LutSin(_motor->angle));
nki 4:fdadf4a3577a 65 _inverter->SetDtcB(LutSin(_motor->angle - 120.0f));
nki 4:fdadf4a3577a 66 _inverter->SetDtcC(LutSin(_motor->angle + 120.0f));
nki 5:ee1e6c84c302 67 */
nki 4:fdadf4a3577a 68
bwang 3:0a2396597e0d 69 float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
bwang 3:0a2396597e0d 70 Clarke(_motor->I_a, _motor->I_b, &alpha, &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 5:ee1e6c84c302 76 _modulator->Update(valpha, vbeta);
nki 5:ee1e6c84c302 77
nki 5:ee1e6c84c302 78
bwang 3:0a2396597e0d 79 }