File content as of revision 1:28d74f044818:

#include "control.h"
#include "threads.h"

cControl::cControl() {
    K[0] = 9.6; K[1] = 2.3; K[2] = -0.12; K[3] = -0.2;
    //K[0] = 10.0; K[1] = 2.4; K[2] = -0.15; K[3] = -0.25;
    q[0] = 24;
    q[1] = -39.999;
    q[2] = 16;
    e[0] = 0;
    e[1] = 0;
    e[2] = 0;
    u_old = 0;
}

void cControl::setCurrent() {
    float temp = 0;
    if(programMode == RUNNING) {
        /* STATE FEEDBACK */
        if(controller == ST_FEEDBACK) {
            temp = -K[0]*states.phi1 - K[1]*states.omega1 - K[2]*states.phi2 - K[3]*states.omega2;
            
            if(temp > 10.0)
                states.current = 10.0;
            
            else {
                
                if(temp < -10.0)
                    states.current = -10.0;
                
                else
                    states.current = temp;
            }
        }
        /* PID CONTROLLER */
        else {   
            float temp;
            e[0] = -states.phi1;
            temp = u_old  + q[0]*e[0] + q[1]*e[1] + q[2]*e[2];
            if(temp > 10.0)
                   states.current = 10.0;
            else {
                if(temp < -10.0)
                    states.current = -10.0;
                else
                    states.current = temp;
            }
            
            e[1] = e[0];
            e[2] = e[1];
            u_old = states.current;
        }
    }
    else
        states.current = 0.0;
}