File content as of revision 2:1d374991b3be:

#include "mbed.h"

template<std::size_t N, std::size_t M, std::size_t P>
int mult(int A[N][M], int B[M][P]) {

    int C[N][P];

    for (int n = 0; n < N; n++) {
        for (int p = 0; p < P; p++) {
            int num = 0;
            for (int m = 0; m < M; m++) {
                num += A[n][m] * B[m][p];
            }
            C[n][p] = num;
            std::cout << num << " ";
        }
        std::cout << std::endl;
    }

    return 0;
    
}

AnalogIn pot1(A0); // X-coord knob
AnalogIn pot2(A3); // Y-coord knob

int main()
{
   const double  L1 = 0.328;
   const double L2 = 0.218:
    
  double  q1 = 0;
  double  q2 =-q1 +0;
    
   double T1[3][3] = {
        {0, -1, 0},
        {1, 0, 0,},
        {0, 0, 0,}};
  double  T20[3][3] = {
        {0, -1, 0},
        {1, 0, -L1,},
        {0, 0, 0,}};
  double  H200[3][3] = {
        {1, 0, L1+L2},
        {0, 1, 0,},
        {0, 0, 1,}};
  double Pe2 [3][1]={
            {0},
            {0},
            {1}};
    double q1 = q1*pi/180;
    double q2 = q2*pi/180;
    
    while (true) {
       double  Pe_set[1][3] = {             // defining setpoint location of end effector
                        {pot1*0.546},             //setting xcoord to pot 1
                        {pot2*0.4},             // setting ycoord to pot 2
                        {1}};
                        
//Calculating new H matrix 
  double T1e=[3][3] = {
        {cos(q1), -sin(q1), 0},
        {sin(q1), cos(q1), 0},
        {0, 0, 1}};
  double  T20e[3][3] = {
        {cos(q2), -sin(q2), L1-L1*cos(q2)},
        {sin(q2), cos(q2), -L1*sin(q2)},
        {0, 0, 1}};
        
  double  H20 = mult<3,3,3>(T1e,T20e);  // matrix multiplication
  double Pe0 = mult<3,3,1>(H20,Pe2);   // matrix multiplication
  double pe0x =Pe0[1];                 // seperating coordinates of end effector location
  double pe0y = Pe[2];
  
  // Determing the jacobian
  
    double T_1[3][1] = {
                {1},
                {T1[1][3]},
                {T1[2][3]};
                
    double T_2[3][1] = {
                {1},
                {L1*sin(q1)},
                {-L1*cos(q1)}};
                
    double J[3][2] = {
             {T_1[1][1], T_2[1][1]},
             {T_1[2][1], T_2[2][1]},
             {T_1[3][1], T_2[3][1]}};
             
//Determing 'Pulling" force to setpoint

    double k= 1;     //virtual stifness of the force

    double Fs[3][1] = {                                     //force vector from end effector to setpoint
                {k*Pe_set[1][1] - k*Pe0[1][1]},
                {k*Pe_set[2][1] - k*Pe0[2][1]},
                {k*Pe_set[3][1] - k*Pe0[3][1]}};
                
    double Fx = Fs[1][1];
    double Fy = Fs[2][1];
    double W0t[3][1] = {
                {Pe0x*Fy - Pe0y*Fx}, 
                {Fx}, 
                {Fy}};
        
                
    double Jt[2][3] = {                                     // transposing jacobian matrix
                {J[1][1], J[1][2], J[1][3]},
                {J[2][1], J[2][2], J[2][3]}};
                
    double tau_st = mult<2,3,1>(Jt,W0t);
    
//Calculating joint behaviour

    double b =1;                    //joint friction coefficent
    double Ts = 1/1000;               //Time step to reach the new angle
    double w_s = tau_st/b;          // angular velocity
    double q1 = q1 +w_s*Ts;         // calculating new angle of q1 in time step Ts
    double q2 = q2 + w_s*Ts;        // calculating new angle of q2 in time step Ts
                
        }
    
    }