N K
/
analoghalls6
motor spins
Fork of analoghalls5 by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 12:15:05 by 1.7.2