File content as of revision 18:c5b408405e3d:

#include "EMG.h"
#include "Motor.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "iostream"
DigitalIn a(D3);    //buttons for testing
DigitalIn b(D2);
AnalogIn pot1(A0); //potmeters for testing
AnalogIn pot2(A1);
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:
int angle_start1 = 100;
int angle_start2 = 100;

Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  50 , 0.6 , 0 , angle_start1 , 0.001  , 0.1, 0);
Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  30 , 0.5 , 0 , angle_start2 , 0.001 , 0.1 , 0 );


/*****************************************************/
// 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();
   // scope.set(0 ,emg_right-emg_left);
    if (1)//fabs(emg_right - emg_left) < 0.008 )
    {
        return 0;
    }
    else
    {
        if ( emg_right - emg_left > 0)
            return 0.1 ;
        else 
            return -0.1 ;    
    }   
}


double get_Y_control_signal(){
    double emg_right = EMG_tri_r.filter();
    double emg_left =  EMG_tri_l.filter();
    //scope.set(1 ,emg_right-emg_left);
    if (fabs(emg_right - emg_left) < 0.008 )
    {
        return 0;
    }
    else
    {
        if ( emg_right - emg_left > 0)
            return 0.1 ;
        else 
            return -0.1 ;    
    }   
}


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

    float time_step = 1;            //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;
    float theta_2 = 2*3.14*motor2.set_angle()/360;                      //get the angles 
    float speed_X_axis = 0.1;//get_X_control_signal();
    float speed_Y_axis = 0;//get_Y_control_signal();                           //get the desired velocitys
    static float q_setpoint1 = 2*3.14*angle_start1/360;
    static float q_setpoint2 = 2*3.14*angle_start2/360; //define the setpoint for motor 1 and 2
   
    double x_poss = L2*sin(theta_1 + theta_2) + L1*cos(theta_1);
    double y_poss = L1*sin(theta_1) - L2*cos(theta_1 + theta_2);
    
    double radius = sqrt(x_poss * x_poss + y_poss * y_poss) ;
    
    
    if( fabs(q_setpoint1*360/(2*3.14) - theta_1*360/(2*3.14)) <= 1 and fabs(q_setpoint2*360/(2*3.14) -theta_2*360/(2*3.14)) <= 1 )
    {
     
        if( cos(theta_2) >= 0 and 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 and 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));        
        }
    if(q_setpoint1*360/(2*3.14)>100)
    {
     q_setpoint1 = (100.0/360)*2*3.14;    
    }
    
    if(q_setpoint1*360/(2*3.14)<50)
    {
     q_setpoint1 = (50.0/360)*2*3.14;   
    }
    if(q_setpoint2*360/(2*3.14)>140)
    {
     q_setpoint2 = (140.0/360)*2*3.14;    
    }
    
    if(q_setpoint2*360/(2*3.14)<100)
    {
     q_setpoint2 = (100.0/360)*2*3.14;   
    }    
    
    
    }
    
    
    scope.set(0, theta_1*360/(2*3.14));     
    scope.set(1, theta_2*360/(2*3.14)); 
    scope.set(2, q_setpoint1*360/(2*3.14));     
    scope.set(3, q_setpoint2*360/(2*3.14));  
    if (!a){
    scope.set(4, q_setpoint1*360/(2*3.14)-theta_1*360/(2*3.14));//motor1.Control_angle(5.0));
    scope.set(5, q_setpoint2*360/(2*3.14)-theta_2*360/(2*3.14));//motor2.Control_angle(150.0));              
    }
    else{
    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.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.033;
            wait(0.1);
            }
        if(b==0){
            cont-=0.033;
            wait(0.1);
            }
    }
    
    
    //return 0;
}