Bayley Wang
/
analoghalls5
wut
Diff: loopdriver.cpp
- Revision:
- 3:0a2396597e0d
- Parent:
- 1:1f58bdcf2956
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/loopdriver.cpp Mon Mar 02 01:24:37 2015 +0000 @@ -0,0 +1,69 @@ +#include "includes.h" +#include "core.h" +#include "sensors.h" +#include "meta.h" +#include "lut.h" + +Inverter* LoopDriver::_inverter; +Motor* LoopDriver::_motor; +User* LoopDriver::_user; +PidController* LoopDriver::_pid_d; +PidController* LoopDriver::_pid_q; +ReferenceSynthesizer* LoopDriver::_reference; +Modulator* LoopDriver::_modulator; + +LoopDriver::LoopDriver(Inverter *inverter, Motor *motor, User *user, PidController *pid_d, + PidController *pid_q, Modulator *modulator, float max_phase_current, int update_frequency) { + _inverter = inverter; + _motor = motor; + _user = user; + _pid_d = pid_d; + _pid_q = pid_q; + _reference = new SynchronousReferenceSynthesizer(max_phase_current);; + _modulator = modulator; + _max_phase_current = max_phase_current; + _update_frequency = update_frequency; +} + +void LoopDriver::Start() { + _upd_ticker.attach_us(&update, 1000000 / _update_frequency); +} + +void LoopDriver::Clarke(float a, float b, float *alpha, float *beta) { + *alpha = a; + *beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b; +} + +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; +} + +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 LoopDriver::LutSin(float theta) { + if (theta < 0.0f) theta += 360.0f; + if (theta >= 360.0f) theta -= 360.0f; + return sinetab[(int) theta]; +} + +float LoopDriver::LutCos(float theta) { + return LutSin(90.0f - theta); +} + +void LoopDriver::update() { + float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta; + Clarke(_motor->I_a, _motor->I_b, &alpha, &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); + InverseParke(vd, vq, _motor->angle, &valpha, &vbeta); +}