File content as of revision 7:806b08205b25:

#include "RSTController.hpp"

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

void RSTController::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];
    }
    
    cmd = outputPWM;
}

void RSTController::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];
}

RSTController& RSTController::operator=(const RSTController& rst) {
	
	this->ordR = rst.ordR;
	this->ordS = rst.ordS;
	this->ordT = rst.ordT;

	int i;
	for(i = 0; i<ordR+1; i++)	this->R[i] = rst.R[i]; 	
	for(i = 0; i<ordS+1; i++)	this->S[i] = rst.S[i]; 	
	for(i = 0; i<ordT+1; i++)	this->T[i] = rst.T[i]; 	
	
	return *this;
}


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