File content as of revision 12:21441b04ba29:

 #include "mbed.h"
#include "math.h"
#include "BiQuad.h"
#include <string>
#include "QEI.h"

//----------------- INITIAL -------------------------
QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
QEI Encoder2(D2,D3,NC,64,QEI::X2_ENCODING);
Ticker EncoderTicker;

DigitalOut motor1direction(D7);
PwmOut motor1control(D6);
 
DigitalOut motor2direction(D4);
PwmOut motor2control(D5);

Serial pc(USBTX, USBRX);
 
    double translatie;
    double hoekgraad;
    double hoekgraad2;
    double hoekrad2;
    double q_dot_angle;
    double q_dot_L;
    double q_dot_q2;
    double J_inv_1_1;
    double J_inv_1_2;
    double J_inv_2_1;
    double J_inv_2_2;
    double p_old_x;
    double p_old_y;
    
    const float pi = 3.141592653589793;     // Value of pi
    double gearratio = 3.857142857;
    double radiuspulley = 0.015915;         // Radius pulley [m]
      
    double K_v = 2;                      //velocity constant, max 6.667 ?
    double L0 = 0.09;                       // starting length


//-----------------GET ENCODER VALUES -------------------------
void EncoderFunc() {
hoekgraad = Encoder1.getPulses() * 0.0857142857;        
// double hoekrad = hoekgraad * 0.0174532925;
hoekgraad2 = Encoder2.getPulses() * 0.0857142857;                // Angle arm [degree]
hoekrad2 = hoekgraad2 * 0.0174532925;
//  double hoekarm = hoekgraad2 / gearratio;
translatie = (hoekgraad / 180.0) * pi * radiuspulley;     // Translatie arm [m]

//---------------- INVERSE KINEMATICS ---------------------------
p_old_x = (translatie+L0)*cos(hoekrad2);          // Everytime the x-value from encoder calculated
p_old_y = (translatie+L0)*sin(hoekrad2);          // Everytime the y-value from encoder calculated

J_inv_1_1 = -sin(hoekrad2)/(translatie+L0);       // Construction of inverse Jacobian
J_inv_1_2 = cos(hoekrad2)/(translatie+L0);
J_inv_2_1 = cos(hoekrad2);
J_inv_2_2 = sin(hoekrad2);    
}


int main()
{   
motor2direction = false;        // Nu staan motoren toch op het begin allebei in positieve stand?
motor2direction = false;
    
EncoderTicker.attach(&EncoderFunc, 0.02);

// Demo path: rectangular
double x_path[2];       // Matrix heeft 5 elementen: beginnend vanaf element 0 tot en met element 4
x_path[0] = L0;
x_path[1] = L0;

double y_path[2];
y_path[0] = 0.0;
y_path[1] = 0.10;

// for loop


    for(int i=0 ; i<=1 ; i++)
    {
    double p_new_x = x_path[i];
    double p_new_y = y_path[i];
    
    double p_dot_new_x = K_v * (p_new_x - p_old_x); //Snelheid is constante K_V * distance(p_old p_new)
    double p_dot_new_y = K_v * (p_new_y - p_old_y);
    
   // printf("x=%f , y=%f , p_dot_new_x=%f , p_dot_new_y=%f\n",p_new_x,p_new_y,p_dot_new_x,p_dot_new_y);
    
    
    double angle_new = atan2(p_new_y,p_new_x)*180/pi;
    double L_new = sqrt(pow(p_new_x,2)+pow(p_new_y,2));
    
    
        if (angle_new - hoekrad2 <= 0) // als hoekveranding ccw > motor cw > true
        {
        motor2direction = true;
        }
        else
        {
        motor2direction = false;
        }      
    
        if (L_new - translatie <= 0 )// als lengteverandering negatief > to base (ccw) > true
        {
        motor1direction = true;
        }
        else
        {
        motor1direction = false;
        }
    
        while ( (fabs(p_new_x - p_old_x)) > 0.05 && (fabs(p_new_y - p_old_y)) > 0.05 )
        {
        q_dot_angle = (J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y);     //hoekrad2
        q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y;         //translatie
        q_dot_q2 = (q_dot_L/radiuspulley);                                     //hoekrad (translatie) in radialen
        motor1control.write(q_dot_q2);
        wait(0.1);
        motor2control.write(q_dot_angle);
        wait(0.1); // %%%Berekening niet tegelijk, eventuele fout? %%%   
        //Printie op pc
        pc.baud(115200);
        pc.printf("q_dot_L = %f\r\n", q_dot_L );
        pc.printf("q_dot_angle = %f\r\n", q_dot_angle );
        pc.printf("J_inverse = \t %f , %f \r\n \t\t %f , %f \r\n",  J_inv_1_1 , J_inv_1_2 , J_inv_2_1 , J_inv_2_2 );
        pc.printf("hoekgraad_trans = %f, translatie = %f, hoekgraad2 = %f \r\n", hoekgraad , translatie, hoekgraad2);
        }       // End of while
    }           // End of for     
}               // End of main()