File content as of revision 26:292492b885f7:

#include "QEI.h"
#include "mbed.h"
#include "Matrix.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "MatrixMath.h"

volatile double theta_1;
//volatile float  theta_error_1;
volatile float  theta_reference_1;
volatile double theta_2;
//volatile float  theta_error_2;
volatile float  theta_reference_2;
const float l = 0.535;
const float EMGx_velocity=0.02;
const float EMGy_velocity=0;
float Ts    = 0.001;
float Kp;
float Ki;
float Kd;
QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
float theta;
float thetav_1;
float thetav_2;

//float ReadEncoder()
//{
//    theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
//    theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
//    float theta[2][1] = {{theta_1},{theta_2}};
//    return theta
//}

float Controller(float theta_error, bool motor)
{
    if (motor == false) {
        float K     = 1;
        float ti    = 0.1;
        float td    = 10;
        Kp    = K*(1+td/ti);
        Ki    = K/ti;
        Kd    = K*td;
    } else {
        float K     = 1;
        float ti    = 0.1;
        float td    = 10;
        Kp    = K*(1+td/ti);
        Ki    = K/ti;
        Kd    = K*td;
    }
    static float error_integral = 0;
    static float error_prev = 0;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // Proportional part:
    float torque_p = Kp * theta_error;

    // Integral part:
    error_integral = error_integral + theta_error * Ts;
    float torque_i = Ki * error_integral;

    // Derivative part:
    float error_derivative = (theta_error - error_prev)/Ts;
    float filtered_error_derivative = LowPassFilter.step(error_derivative);
    float torque_d = Kd * filtered_error_derivative;
    error_prev = theta_error;

    // Sum all parts and return it
    float torque = torque_p + torque_i + torque_d;
    return torque;
}

void AngleVelocity(float theta_1,float theta_2)
{
    float DET_jacobian= 1.0f/((-l*sin(theta_1)-l*sin(theta_1+theta_2))*(l*cos(theta_1+theta_2))-(-l*sin(theta_1+theta_2))*(l*cos(theta_1)+l*cos(theta_1+theta_2)));
    float INV_jacobian[2][2]={{DET_jacobian*l*cos(theta_1+theta_2) , DET_jacobian*l*sin(theta_1+theta_2)} , {DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2) , DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)}};   
    thetav_1=DET_jacobian*l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*l*sin(theta_1+theta_2)*EMGy_velocity;
    thetav_2= DET_jacobian*-l*cos(theta_1)-l*cos(theta_1+theta_2)*EMGx_velocity + DET_jacobian*-l*sin(theta_1)-l*sin(theta_1+theta_2)*EMGy_velocity;
}

int main(void)
{
    while(1)
    {
    AngleVelocity(0.2f,0.0f); 
    pc.printf('%f \n %f /n/r',thetav_1, thetav_2);
        }  
}