File content as of revision 4:fc5889cf69f9:

// ------------- INITIALIZING -----------------

#include "mbed.h"
#include "BiQuad.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "Math.h"

AnalogIn button2(A4);
PwmOut pwmpin1(D6);
DigitalOut directionpin1(D7);
QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
Ticker ref_rot;
Ticker show_counts;
Ticker Scope_Data;
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(2);

// INITIALIZING GLOBAl VALUES
double Kp = 100; //200 , 50
double Ki = 10;   //1, 0.5
double Kd = 0; //200, 10
double Ts = 0.1; // Sample time in seconds
volatile double reference_rotation = 3.14f; //define as radians
double motor_position;
bool AlwaysTrue;
double PI = 3.141592653589793238462;

//----------- FUNCTIONS ----------------------


// PID CONTROLLER FUNCTION
double PID_controller(double error)
{
    static double error_integral = 0;
    static double error_prev = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // Proportional part:
    double u_k = Kp * error;

    // Integral part
    error_integral = error_integral + error * Ts;
    double u_i = Ki * error_integral;

    // Derivative part
    double error_derivative = (error - error_prev)/Ts;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error;

    // Sum all parts and return it
    return u_k + u_i + u_d;
}


// DIRECTON AND SPEED CONTROL
void moter_control(double u)
{
    directionpin1= u > 0.0f; //eithertrueor false
    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
}


// CONTROLLING THE MOTOR
void Motor_mover()
{
    double motor_position = encoder1.getPulses(); //output in counts
    double error = reference_rotation - motor_position*(2*PI)/8400;
    double u = PID_controller(error);
    moter_control(u);
}

//PRINT TICKER
void PrintFlag()
{
    AlwaysTrue = true;
}

// HIDSCOPE
void ScopeData()
{
    double y = encoder1.getPulses();
    scope.set(0, y);
    scope.send();
}

//------------------ EXECUTION ---------------

int main()
{
    // INITIALIZING

    pwmpin1.period_us(60);
    pc.printf("test");
    pc.baud(9600);
    //show_counts.attach(PrintFlag, 0.2);
    ref_rot.attach(Motor_mover, 0.01);
    Scope_Data.attach(ScopeData, 0.1);

// DEFININING VARIABLES
    //float reference;
    float foo[17];
    float length = 9.0f;
    int a;
    int q;

while (true) {

    // EXECTURION  IN MAIN
        // SIN ALS INPUT GEVEN
        if (button2 == false){
            wait (0.2f);
        for (q=0; q<4; q++){
            float b = 8.0f;
            for (a=0; a<10; a++){
                foo[a] = 3.14f/length*(a);
                //printf("%f\n\r", foo[a]);
                reference_rotation = foo[a];
                wait(0.1f);
                }
            for(a=10; a<18; a++){
                foo[a] = 3.14f/length*b;
                //printf("%f\n\r", foo[a]);
                reference_rotation = foo[a];
                b = b-1.0f;
                wait(0.1f);
                }
            }

        reference_rotation = PI;



        /* //IETS PRINTEN OP JE TERMINAL
        PrintFlag();
        while (AlwaysTrue){
            float q = encoder1.getPulses();
            pc.printf("%f \r\n", q);
            AlwaysTrue = false;
            wait(0.2f);
            }
            */
            
            

    }

}
}