File content as of revision 12:69a9cf74583e:

#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 ,  50 , 0.3 );
Motor motor1(D11 , D10 , D4 , D5  , 50000 ,  50 , 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();
                                                 // time_stepODO: time_stepune 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();
                                                 // time_stepODO: `time_stepune emg to velocity mapping    
    return cont;// emg_fwd - emg_bwd;  
    
}

/******************************************************/
//set speed of setpoints
void control_motors()
{

    float time_step = 0.002;            //set the sample time
    float threshold = 0.01;     //set the threshold for cos(theta_2)
    float L1 = 0.48, L2 = 0.84; //set the lenght of arm 1 and 2
    float theta_1 = 2*3.14*motor1.set_angle()/360 , theta_2 = 2*3.14*motor2.set_angle()/360;                      //get the angles 
    float speed_X_axis = 0;//get_X_control_signal()
    float speed_Y_axis = get_Y_control_signal();                           //get the desired velocitys
    float q_setpoint1, q_setpoint2; //define the setpoint for motor 1 and 2
    
    if((cos(theta_2))>=0 && cos(theta_2)<threshold){

    q_setpoint1=theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*(threshold));
    q_setpoint2=theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*(threshold));

    }
    else if((cos(theta_2))<0 && cos(theta_2)>-threshold){

    q_setpoint1=theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*(-threshold));
    q_setpoint2=theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*(-threshold));
    }
    else{
    q_setpoint1=theta_1 + (time_step*(speed_Y_axis*cos(theta_1 + theta_2) - speed_X_axis*sin(theta_1 + theta_2)))/(L1*cos(theta_2));
    q_setpoint2=theta_2 + (time_step*(L1*speed_Y_axis*sin(theta_1) - L2*speed_Y_axis*cos(theta_1 + theta_2) + L2*speed_X_axis*sin(theta_1 + theta_2) + L1*speed_X_axis*cos(theta_1)))/(L1*L2*cos(theta_2));        
    }
    
    
    
    scope.set(0, theta_1*360/(2*3.14));
    scope.set(1, cont);     
    scope.set(2, theta_1*360/(2*3.14));
    scope.set(3, speed_Y_axis);                    
    scope.set(4, motor1.Control_angle(q_setpoint1*360/(2*3.14)) );
    scope.set(5, motor2.Control_angle(q_setpoint2*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.01;
            wait(0.1);
            }
        if(b==0){
            cont-=0.01;
            wait(0.1);
            }
    }
    
    
    //return 0;
}