File content as of revision 11:dd1976534a03:

#include "EMG.h"
#include "Motor.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "iostream"
DigitalIn a(D3);    //buttons for testing
DigitalIn b(D2);
double cont = 0 ;

HIDScope scope(6); // 4 channels of data
Ticker MainTicker;
MODSERIAL pc(USBTX, USBRX);

/*****************************************************************/
//Initialize Analog EMG inputs:

    
EMG EMG_bi_r(A0);                            // Move  the endpoint to the right (plus direction)
EMG EMG_bi_l(A1);                            // Move the endpoint to the left (minus direction)
EMG EMG_tri_r(A2);                           // Move the endpoint forward (plus direction)
EMG EMG_tri_l(A3);                           // Move the endpoint backward (minus direction)

/****************************************************/
//Initialise Motors:

Motor motor2(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.5 );
Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  180 , 0.5 );

/*****************************************************/
// Set control signals:

//x direction is the righ/left movement
//y direction is forward/backward movement

double get_X_control_signal(){
    double emg_right = EMG_bi_r.filter();
    double emg_left =  EMG_bi_l.filter();
                                                 // TODO: Tune emg to velocity mapping
    return  emg_right - emg_left;
    
}


double get_Y_control_signal(){
    double emg_fwd= EMG_tri_r.filter();
    double emg_bwd= EMG_tri_l.filter();
                                                 // TODO: `Tune emg to velocity mapping    
    return cont;// emg_fwd - emg_bwd;  
    
}

/******************************************************/
//set speed of setpoints
void control_motors()
{
    int row_J =2 , row_Speed=2 , column_J =2;
    float speed_setpoint[row_J] , J_inv[row_J][column_J] , speed[row_Speed];
    
    speed[0] = 0;//get_X_control_signal();
    speed[1] = get_Y_control_signal();
    
    float theta_1 = 2*3.14*motor1.set_angle()/360 , theta_2 = 2*3.14*motor2.set_angle()/360; 
    float L1 = 0.48;
    float L2 = 0.84;
    
    J_inv[0][0] = -sin(theta_1 + theta_2)/(L1*cos(theta_2)) ;
    J_inv[0][1] = cos(theta_1 + theta_2)/(L1*cos(theta_2))  ;
    J_inv[1][0] = (L2*sin(theta_1 + theta_2) + L1*cos(theta_1))/(L1*L2*cos(theta_2))  ;
    J_inv[1][1] = -(L2*cos(theta_1 + theta_2) - L1*sin(theta_1))/(L1*L2*cos(theta_2))  ;
    
    // Initializing elements of matrix mult to 0.
    for(int i = 0; i < row_J; ++i)
    {
        speed_setpoint[i] = 0;
    }

    // Multiplying matrix firstMatrix and secondMatrix and storing in array mult.
    for(int i = 0; i < row_J; ++i)
    {
        for(int k=0; k<column_J; ++k)
        {
             speed_setpoint[i] += J_inv[i][k] * speed[k];
        }
    }
     float time = 0.002 ; 
    
    scope.set(0, theta_1*360/(2*3.14));
    scope.set(1, cont);
    scope.set(2, theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14));
    scope.set(3, theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14));
    
   
                               
    scope.set(4, motor1.Control_angle(theta_1*360/(2*3.14) + speed_setpoint[0]*time*360/(2*3.14)) );
    scope.set(5, motor2.Control_angle(theta_2*360/(2*3.14) + speed_setpoint[1]*time*360/(2*3.14)) );
    
}

/******************************************************/
// Ticker Function:

void mainTicker()
{    
    
    
    control_motors();
    

   // scope.set(0, x_control_signal);
   // scope.set(1, motor2.set_angle());
    scope.send();
    
}


/***************************************************/
//Main Function:

int main(void)
{
    
    double sample_time= 0.002;                            //fs = 500Hz
    pc.baud(115200);                                      //Set Baud rate for Serial communication
    MainTicker.attach(&mainTicker, sample_time);          //Attach time based interrupt
    
  
    while(true)
    {
        if(a==0){
            cont+=0.1;
            wait(0.1);
            }
        if(b==0){
            cont-=0.1;
            wait(0.1);
            }
    }
    
    
    //return 0;
}