File content as of revision 21:59431788a42d:

#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 ;
bool test;

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 = 90;//51.3676;
int angle_start2 = 90;//140.2431;

Motor motor1(D13 , D12 , D7 , D6  , 50000 ,  180 , 0.7 , 0 , angle_start1 , 5 , 1, 0.00000);
Motor motor2(D11 , D10 , D4 , D5  , 50000 ,  90 , 0.7 , 0 , angle_start2 , 7 , 1 , 0.00000 );


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


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

    
    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 , speed_Y_axis = 0 ;
    if (test)
        speed_X_axis = get_X_control_signal();
    else
        speed_Y_axis = get_X_control_signal();                           //get the desired velocitys
    
    
    
    
    scope.set(0, theta_1*360/(2*3.14));     
    scope.set(1, theta_2*360/(2*3.14));// 
    scope.set(2, speed_X_axis);     
    scope.set(3, speed_Y_axis); 
    scope.set(4, motor1.Control_angle(theta_1*360.0/(2*3.14) + speed_X_axis*5));
    scope.set(5, motor2.Control_angle(theta_2*360.0/(2*3.14) + speed_Y_axis*5));  
    
        
}

/******************************************************/
// 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){
            test=!test;
            wait(0.5);
            }
        if(b==0){
            cont-=0.033;
            wait(0.1);
            }
    }
    
    
    //return 0;
}