File content as of revision 13:559f8946f16d:

#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:
int angle_start1 = 80;
int angle_start2 = 100;

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

/*****************************************************/
// 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: time_stepune emg to velocity mapping
    return 0;// emg_right - emg_left;
    
}


double get_Y_control_signal(){
    double emg_fwd= EMG_tri_r.filter();
    double emg_bwd= EMG_tri_l.filter();
                                                 // TODO: `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.05;     //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 = get_X_control_signal();
    float speed_Y_axis = 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( radius < (L1 + L2) )
    {
     
        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));        
        }
    }
    
    
    scope.set(0, theta_1*360/(2*3.14));
    scope.set(1, theta_2*360/(2*3.14));
    scope.set(2, cont);     
    scope.set(3, radius);  
    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;
}