File content as of revision 3:710f993d5848:

#include "mbed.h"
//#include "HIDScope.h"
//#include "QEI.h"
#include "MODSERIAL.h"
//#include "BiQuad.h"
//#include "FastPWM.h"
#define M_PI 3.14159265358979323846  /* pi */
#include <math.h>

double vx = 5;                                              //Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
double vy = 3;                                              //Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)

void Inverse_Kinematics()                              
    {
      // Defining variables used for Brockett
     const double x_coordinate_in_reference = 0.14;        //Nog invullen!! x-coordinate endpoint in reference (m)
     const double y_coordinate_in_reference = 0;           //Nog invullen!! y-coordinate endpoint in reference (m)

     const double phi_d = 20;                              //Nog invullen!! Absolute angle joint 1 in reference (deg)
     const double phi = phi_d*M_PI/180;                    //(rad)

     // Defining motor angles
     double M1 = 20*M_PI/180;                              //Vervang 20 door encoder_value_motor_1 (deg), de naam die is gegeven aan hoek die uit encoder counts is gekomen, current encoder position
     double M2 = 30*M_PI/180;                             //Zelfde als hierboven, current encoder position

     // Calculating joint angles based on motor angles (current encoder values)
     double q1 = M1;                                        //Relative angle joint 1 (rad)
     double q2 = M2 - M1;                                   //Relative angle joint 2 (rad) 
     
     // Defining variables used for twists in Jacobian
     const double x1 = 0;                                  //x-position of motors (first joint) from the bottom coordinate system (m)
     const double y1 = 0.148;                              //y-position of motors (first joint) from the bottom coordinate system (m)
     
     double x2 = x1 + 0.425*cos(phi+q1);                       //x-position of the second joint expressed in the angle of the first joint (m)
     double y2 = y1 + 0.425*sin(phi+q1);                       //y-position of the second joint expressed in the angle of the first joint (m)
     
     // Calculating end-effector position from joint angles with Brockett
     // Defining H-matrix in reference from end-effector coordinate system to 0
     double H0e_0[3][3];
     H0e_0[0][0] = 1.0;
     H0e_0[0][2] = x_coordinate_in_reference;
     H0e_0[1][1] = 1.0;
     H0e_0[1][2] = y_coordinate_in_reference;
     H0e_0[2][2] = 1.0;  
     
     // Defining variables used for twists in Brockett
     const double x2_brockett = x1 + 0.425*cos(phi);
     const double y2_brockett = y1 + 0.425*sin(phi);
                           
     // Calculating Brockett
     double exponent_1[3][3];
     exponent_1[0][0] = cos(q1);
     exponent_1[0][1] = -sin(q1);
     exponent_1[0][2] = x1-x1*cos(q1)+y1*sin(q1);
     exponent_1[1][0] = sin(q1);
     exponent_1[1][1] = cos(q1);
     exponent_1[1][2] = y1-y1*cos(q1)-x1*sin(q1);
     exponent_1[2][2] = 1.0;
     
     double exponent_2[3][3];
     exponent_2[0][0] = cos(q2);
     exponent_2[0][1] = -sin(q2);
     exponent_2[0][2] = x2_brockett-x2_brockett*cos(q2)+y2_brockett*sin(q2);
     exponent_2[1][0] = sin(q2);
     exponent_2[1][1] = cos(q2);
     exponent_2[1][2] = y2_brockett-y2_brockett*cos(q2)-x2_brockett*sin(q2);
     exponent_2[2][2] = 1.0;
     
     double H0e[3][3];
     H0e = exponent_1[3][3]*exponent_2[3][3];
    }

int main()
{

}