File content as of revision 9:d5c561b3ea5a:

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

//-----------------GET ENCODER VALUES -------------------------
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);

InterruptIn button1(D10);

Serial pc(USBTX, USBRX);
 
 
 double translatie;
 double hoekgraad;
 double hoekgraad2;
 
     
    const float pi = 3.141592653589793;                             // Value of pi
    double gearratio = 3.857142857;
    double radiuspulley = 0.015915;                                 // Radius pulley [m]

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

//----------------INVERSE KINEMATICS ---------------------------
double K_v = 1.00; //velocity constant, max 6.667
double L0 = 0.09; // starting length

int main()
{   
EncoderTicker.attach(&EncoderFunc, 0.005);
pc.baud(115200);
pc.printf("translatie:%f /n hoekgraad:%f /n hoekgraad2:%f", translatie, hoekgraad, hoekgraad2);


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

double J_inv_1_1 = -sin(hoekgraad)/translatie;            // Construction of inverse Jacobian
double J_inv_1_2 = cos(hoekgraad)/translatie;
double J_inv_2_1 = cos(hoekgraad);
double J_inv_2_2 = sin(hoekgraad);


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

    double y_path[5];
    y_path[0] = 0.0;
    y_path[1] = 0.1;
    y_path[2] = 0.1;
    y_path[3] = 0.0;
    y_path[4] = y_path[0];

// for loop


for(int i=0 ; i<=4 ; 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_old = atan(p_old_y/p_old_x)*180/pi; //Dynamische manier om hoek en lengte verandering te bepalen
    double L_old = sqrt(pow(p_old_x,2)+pow(p_old_y,2));
    
    double angle_new = atan(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 - angle_old <= 0) // als hoekveranding ccw > motor cw > true
        {
        motor2direction = true;
        }
        else
            {
            motor2direction = false;
            }      
    
      if (L_new - L_old <= 0 )// als lengteverandering negatief > to base (ccw) > true
        {
        motor1direction = true;
        }
        else
            {
            motor1direction = false;
            }
    
        while ( (fabs(p_new_x - p_old_x)) > 0.005 || (fabs(p_new_y - p_old_y)) > 0.005 )
        {
            double q_dot_angle = J_inv_1_1 * p_dot_new_x + J_inv_1_2 * p_dot_new_y;     //hoekgraad1
            double q_dot_L = J_inv_2_1 * p_dot_new_x + J_inv_2_2 * p_dot_new_y;         //translatie
            double q_dot_q2 = q_dot_L/radiuspulley;                                     //hoekgraad2 (translatie)
            motor1control.write(q_dot_angle);
            wait(0.1);
            motor2control.write(q_dot_q2);
            wait(0.1); // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Berekening niet tegelijk, eventuele fout? %%%
        }
    }
}