loopdriver.cpp

00001 #include "includes.h"
00002 #include "core.h"
00003 #include "sensors.h"
00004 #include "meta.h"
00005 #include "lut.h"
00006 
00007 Inverter* LoopDriver::_inverter;
00008 Motor* LoopDriver::_motor;
00009 User* LoopDriver::_user;
00010 PidController* LoopDriver::_pid_d;
00011 PidController* LoopDriver::_pid_q;
00012 ReferenceSynthesizer* LoopDriver::_reference;
00013 Modulator* LoopDriver::_modulator;
00014 int LoopDriver::_blinky_toggle;
00015 
00016 LoopDriver::LoopDriver(Inverter *inverter, Motor *motor, User *user, PidController *pid_d, 
00017                        PidController *pid_q, Modulator *modulator, float max_phase_current, int update_frequency) {
00018     _inverter = inverter;
00019     _motor = motor;
00020     _user = user;
00021     _pid_d = pid_d;
00022     _pid_q = pid_q;
00023     _reference = new SynchronousReferenceSynthesizer(max_phase_current);;
00024     _modulator = modulator;
00025     _max_phase_current = max_phase_current;
00026     _update_frequency = update_frequency;
00027 }
00028 
00029 void LoopDriver::Start() {
00030     _upd_ticker.attach_us(&update, 1000000 / _update_frequency);
00031 }
00032 
00033 void LoopDriver::Clarke(float c, float b, float *alpha, float *beta) {
00034     *alpha = b;
00035     //*beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b;
00036     *beta = (b + 2.0f * c)/sqrt(3.0f);
00037 }
00038 
00039 void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) {
00040     float cos = LutCos(theta);
00041     float sin = LutSin(theta);
00042     *d = alpha * cos - beta * sin;
00043     *q = -alpha * sin - beta * cos;
00044 }
00045 
00046 void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) {
00047     float cos = LutCos(theta);
00048     float sin = LutSin(theta);
00049     //*alpha = cos * d - sin * q;
00050     //*beta = -(sin * d + cos * q);  
00051     
00052     *alpha = cos * 1.0f;
00053     *beta = -sin * 1.0f;                                    
00054 }
00055 
00056 float LoopDriver::LutSin(float theta) {
00057     if (theta < 0.0f) theta += 360.0f;
00058     if (theta >= 360.0f) theta -= 360.0f;
00059     return sinetab[(int) theta] * 2.0f - 1.0f;
00060 }
00061 
00062 float LoopDriver::LutCos(float theta) {
00063     return LutSin(90.0f - theta);
00064 }
00065 
00066 void LoopDriver::update() {    
00067     float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
00068     Clarke(_motor->I_c, _motor->I_b, &alpha, &beta);
00069     test_alpha = alpha;
00070     test_beta = beta;
00071     Parke(alpha, beta, _motor->angle, &d, &q);
00072     _reference->GetReference(_motor->angle, &ref_d, &ref_q);
00073     float vd = _pid_d->Update(ref_d, d);
00074     float vq = _pid_q->Update(ref_q, q);
00075     InverseParke(vd, vq, _motor->angle, &valpha, &vbeta);
00076     _modulator->Update(valpha, vbeta);   
00077 }