File content as of revision 4:8f67b8327300:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"

MODSERIAL pc(USBTX, USBRX);
DigitalOut DirectionPin1(D4);
DigitalOut DirectionPin2(D7);
PwmOut PwmPin1(D5);
PwmOut PwmPin2(D6);
DigitalIn Knop1(D2);
DigitalIn Knop2(D3);
AnalogIn pot1 (A5);
AnalogIn pot2 (A4);
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );

Ticker      StateTicker;
Ticker      EncodingTicker;
Ticker      printTicker;
Ticker      EMG_Read_Ticker;
Ticker      sample_timer;
HIDScope    scope( 4 );

volatile float Bicep_Right          = 0.0;
volatile float Bicep_Left           = 0.0;
volatile float Tricep_Right         = 0.0;
volatile float Tricep_Left          = 0.0;
volatile const float maxVelocity    = 8.4; // in rad/s
volatile const double pi            = 3.14159265358979; 
volatile float referenceVelocity1   = 0.5; //dit is de gecentreerde waarde en dus de nulstand
volatile float referenceVelocity2   = 0.5;

enum states{Calibration, Homing, Function};
    
volatile states Active_State = Calibration;

volatile int counts1;
volatile int counts2;
    
void Encoding()
{

    QEI Encoder1(D12,D13,NC,64);
    QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    
    float rad_m1 = ((2*pi)/32.0)* (float)counts1;
    float rad_m2 = ((2*pi)/32.0)* (float)counts2;
    
   // pc.printf("%f  &  %f ....\n",rad_m1, rad_m2);
}

void EMG_Read()
{
    Bicep_Right     =   emg0.read();
    Bicep_Left      =   emg1.read();
    Tricep_Right    =   emg2.read();
    Tricep_Left     =   emg3.read();   
}

void sample()
{
    
    scope.set(0, emg0.read() );
    scope.set(1, emg1.read() );
    scope.set(2, emg2.read() );
    scope.set(3, emg3.read() );
  
    scope.send();
}

   
void velocity1()
    {
            if (pot1.read()>0.5f)
                {
                // Clockwise rotation
                referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; 
                }
            
            else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f))
            {
                referenceVelocity1 = pot1.read() * 0.0f; 
            } 
            
            else if (pot1.read() < 0.5f)
                {
                // Counterclockwise rotation      
                referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
                }
    }
    
void velocity2()
    {
            if (pot2.read()>0.5f)
                {
                // Clockwise rotation
                referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; 
                }
            
            else if (pot2.read() == 0.5f)
            {
                referenceVelocity2 = pot2.read() * 0.0f; 
            } 
            
            else if (pot2.read() < 0.5f)
                {
                // Counterclockwise rotation      
                referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
                }
    }    
    
void motor1()
    {  
        float u = referenceVelocity1;
        DirectionPin1 = u < 0.0f;
        PwmPin1 = fabs(u);
    }

void motor2()
    {  
        float u = referenceVelocity2;
        DirectionPin2 = u > 0.0f;
        PwmPin2 = fabs(u);
    }

void Printing()
{
    float v1 = fabs(referenceVelocity1) * maxVelocity;
    float v2 = fabs(referenceVelocity2) * maxVelocity;
    
    //eventueel nog counts -> rad/s 
    
    //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
    pc.printf("%f ", counts1);
}

void StateMachine() 
{
    switch (Active_State)
    {
        case Calibration:
        //calibration actions
        //pc.printf("Calibration State");
            if (Knop1==false)
            {
                pc.printf("Entering Homing state \n");
                Active_State = Homing;
            }
        break;
        
        case Homing:
        // Homing actions
        //pc.printf("Homing State");
            if (Knop2==false)
            {
                pc.printf("Entering Funtioning State \n");
                Active_State = Function;
            }
        break;
        
        case Function:
        //pc.printf("Funtioning State");
        
            velocity1();
            velocity2();
            motor1();
            motor2();
        break;    
            
        default:
        pc.printf("UNKNOWN COMMAND");
    }
}    

int main()
{
    pc.baud(115200);    
    PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz 
    
    EncodingTicker.attach(&Encoding, 0.002);
    
    sample_timer.attach(&sample, 0.002);
    EMG_Read_Ticker.attach(&EMG_Read, 0.002);
    
    StateTicker.attach(StateMachine, 0.002);
   
    //printTicker.attach(&Printing, 2.0);
    
    while(true)
    {        
    }
}