File content as of revision 10:b4abecccec7a:

#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, _user->throttle, &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);
    
    vd = 0.0f;
    vq = _user->throttle;
    
    
    InverseParke(vd, vq, _motor->angle, &valpha, &vbeta);
    
    if(acquire==1){
        if(bufidx < bufsize){
            fbuffer[bufidx] = _motor->angle;
        }
        else{
            acquire = 0;
        }
    }   
    _modulator->Update(valpha, vbeta); 
    
    /*
    test_alpha = valpha;
    test_beta = vbeta;
    */  
}