working-est copy with class-based code. still open loop
Fork of analoghalls6 by
loopdriver.cpp
- Committer:
- nki
- Date:
- 2015-03-06
- Revision:
- 9:d3b70c15baa9
- Parent:
- 6:99ee0ce47fb2
- Child:
- 10:b4abecccec7a
File content as of revision 9:d3b70c15baa9:
#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; int LoopDriver::_blinky_toggle; 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 c, float b, float *alpha, float *beta) { /* //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 = 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 = FastCos(theta); float sin = FastSin(theta); *alpha = cos * d - sin * q; *beta = sin * d + cos * q; /* *alpha = cos * 1.0f; *beta = -sin * 1.0f; */ } float LoopDriver::LutSin(float theta) { if (theta < 0.0f) theta += 360.0f; if (theta >= 360.0f) theta -= 360.0f; return sinetab[(int) theta] * 2.0f - 1.0f; } 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_c, _motor->I_b, &alpha, &beta); Parke(alpha, beta, _motor->angle, &d, &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); /* _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); */ }