RST.cpp

00001 #include "RST.hpp"
00002 
00003 RST::RST() {
00004     R = 0;
00005     S = 0;
00006     T = 0;
00007 }
00008 
00009 float RST::calculateCmd()
00010 {
00011     static float last_references[ControllerParams::MAX_ORD];
00012     static float last_outputs[ControllerParams::MAX_ORD];
00013     static float last_commands[ControllerParams::MAX_ORD];
00014     
00015     float R_component;
00016     float S_component;
00017     float T_component;
00018     float outputPWM;
00019     
00020     
00021     for(int i=0;i<(ordR+1);i++)
00022     {
00023         last_references[0] = this->ref;
00024         R_component = R_component + R[i]*last_references[i];
00025     }
00026     for(int i=0;i<ordR;i++)
00027     {
00028         last_references[i+1] = last_references[i];
00029     }
00030     for(int i=0;i<(ordT+1);i++)
00031     {
00032         last_outputs[0] = this->out;
00033         T_component = T_component + T[i]*last_outputs[i];
00034     }
00035     for(int i=0;i<ordT;i++)
00036     {
00037         last_outputs[i+1] = last_outputs[i];
00038     }
00039     for(int i=1;i<(ordS+1);i++)
00040     {
00041         S_component = S_component + last_commands[i]*S[i];
00042     }
00043     
00044     outputPWM = T_component - R_component - S_component;
00045     
00046     for(int i=0;i<ordS;i++)
00047     {
00048         last_commands[i+1] = last_commands[i];
00049     }
00050     
00051     return outputPWM; 
00052 }
00053 
00054 void RST::updateParams(ControllerParams &cp) {
00055     ordR = cp.ordR;
00056     ordS = cp.ordS;
00057     ordT = cp.ordT; 
00058 
00059     R = new float[ordR+1];
00060     S = new float[ordS+1];
00061     T = new float[ordT+1];
00062     
00063     int i;
00064     for (i=0; i<ordR+1; i++) R[i] = cp.R[i];
00065     for (i=0; i<ordS+1; i++) S[i] = cp.S[i];
00066     for (i=0; i<ordT+1; i++) T[i] = cp.T[i];
00067 }
00068 
00069 
00070 RST::~RST() {
00071     delete [] R;
00072     delete [] S;
00073     delete [] T;
00074 }