File content as of revision 0:b5906c81772b:

#include "RST.hpp"

RST::RST() {
    R = 0;
    S = 0;
    T = 0;
}

float RST::calculateCmd()
{
    static float last_references[ControllerParams::MAX_ORD];
    static float last_outputs[ControllerParams::MAX_ORD];
    static float last_commands[ControllerParams::MAX_ORD];
    
    float R_component;
    float S_component;
    float T_component;
    float outputPWM;
    
    
    for(int i=0;i<(ordR+1);i++)
    {
        last_references[0] = this->ref;
        R_component = R_component + R[i]*last_references[i];
    }
    for(int i=0;i<ordR;i++)
    {
        last_references[i+1] = last_references[i];
    }
    for(int i=0;i<(ordT+1);i++)
    {
        last_outputs[0] = this->out;
        T_component = T_component + T[i]*last_outputs[i];
    }
    for(int i=0;i<ordT;i++)
    {
        last_outputs[i+1] = last_outputs[i];
    }
    for(int i=1;i<(ordS+1);i++)
    {
        S_component = S_component + last_commands[i]*S[i];
    }
    
    outputPWM = T_component - R_component - S_component;
    
    for(int i=0;i<ordS;i++)
    {
        last_commands[i+1] = last_commands[i];
    }
    
    return outputPWM; 
}

void RST::updateParams(ControllerParams &cp) {
    ordR = cp.ordR;
    ordS = cp.ordS;
    ordT = cp.ordT; 

    R = new float[ordR+1];
    S = new float[ordS+1];
    T = new float[ordT+1];
    
    int i;
    for (i=0; i<ordR+1; i++) R[i] = cp.R[i];
    for (i=0; i<ordS+1; i++) S[i] = cp.S[i];
    for (i=0; i<ordT+1; i++) T[i] = cp.T[i];
}


RST::~RST() {
    delete [] R;
    delete [] S;
    delete [] T;
}