File content as of revision 1:d3e12393b71d:

#include "RST.h"

RST::RST(float* R, float* S, float* T, RST_aditionalData SP):m_R(R), m_S(S), m_T(T), m_SP(SP)
{
    //
}

float RST::ComputeCommand(float inputADC)
{
    static float last_references[MAX_GRADE];
    static float last_outputs[MAX_GRADE];
    static float last_commands[MAX_GRADE];
    float R_component = 0;
    float T_component = 0;
    float S_component = 0;
    float outputPWM;
    
    for(int i=0;i<(m_SP.gradR+1);i++)
    {
        last_references[0] = m_SP.setpoint;
        R_component = R_component + m_R[i]*last_references[i];
    }
    for(int i=0;i<m_SP.gradR;i++)
    {
        last_references[i+1] = last_references[i];
    }
    for(int i=0;i<(m_SP.gradT+1);i++)
    {
        last_outputs[0] = inputADC;
        T_component = T_component + m_T[i]*last_outputs[i];
    }
    for(int i=0;i<m_SP.gradT;i++)
    {
        last_outputs[i+1] = last_outputs[i];
    }
    for(int i=1;i<(m_SP.gradS+1);i++)
    {
        S_component = S_component + last_commands[i]*m_S[i];
    }
    
    outputPWM = T_component - R_component - S_component;
    last_commands[0] = outputPWM;
    
    for(int i=0;i<m_SP.gradS;i++)
    {
        last_commands[i+1] = last_commands[i];
    }
    
    return outputPWM;
}