File content as of revision 8:e8734a254818:

#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);
DigitalIn Knop3(PTA4);
AnalogIn pot1 (A5);
AnalogIn pot2 (A4);
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );
DigitalOut led(LED_GREEN);

QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);

Ticker      StateTicker;
Ticker      printTicker;

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 float pi             = 3.1415926; 
volatile const float rad_count      = 0.0007479; // 2pi/8400;

volatile float referenceVelocity1   = 0.5; //dit is de gecentreerde waarde en dus de nulstand
volatile float referenceVelocity2   = 0.5;

volatile float q_1;
volatile float q_2;
volatile float r_1;
volatile float r_2;
volatile const float r_3 = 0.035;
volatile float w_1;
volatile float w_2;

volatile float Flex_Bi_R;
volatile float Flex_Bi_L;
volatile float Flex_Tri_R;
volatile float Flex_Tri_L;


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

volatile int counts1 ;
volatile int counts2 ;
volatile float rad_m1;
volatile float rad_m2;
    
void Encoding()
{
 
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    
    rad_m1 = rad_count * (float)counts1;
    rad_m2 = rad_count * (float)counts2;
}

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 Inverse()
{
    q_1= rad_m1;                                          // uit Encoder
    q_2= rad_m2+(pi/2.0f);                                          // uit Encoder
    r_1= -0.2f;
    r_2= -0.2f;
    
        float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
        float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
        float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
        float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
        float D = 1.0f/(u*z-x*y);                         // Determinant
        printf("Determinant is %f\n", D);
        
        float a = D*z;                                  // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
        float b = -D*x;                                 // Inverse jacobian
        float c = -D*y;                                 // Inverse jacobian
        float d = D*u;                                  // Inverse jacobian
        
        float vx = 0.01f;                                // uit emg data
        float vy = 0.0f;                                 // uit emg data
        
        w_1 = vx*a+vy*b;
        w_2 = vx*c+vy*d;
        
        /*
        printf("%f\n", w_1);
        printf("%f\n", w_2);
        */     
}
   
void velocity1()
    {
            if (pot1.read()>0.5f)
                {
                // Clockwise rotation
                referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; 
                }
            
            else if (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_v1 = w_1; //referenceVelocity1       
        float u = u_v1/ (2.0f * pi);
        DirectionPin1 = u < 0.0f;
        PwmPin1 = fabs(u);
    }

void motor2()
    {  
        float u_v2 = w_2; //referenceVelocity2
        float u = u_v2/ (2.0f * pi);
        DirectionPin2 = u > 0.0f;
        PwmPin2 = fabs(u);
    }
    
void Calibrating()
{
    static float n = 0.0;
    static float m = 0.0;
    static float l = 0.0;
    static float k = 0.0;
 
    for(int ii=0; ii<=20000; ii++)
    {
            if (ii <2500)
            {
                n = n + emg0.read();
            }
            else if (ii == 2500)
            {
                Flex_Bi_R = n / 2500.0f;
            }
            else if (ii>2500 && ii<=3500)
            {
                //chillen
            }
            else if(ii>3500 && ii<6000)
            {
                m = m + emg1.read();
            }
            else if (ii == 6000)
            {
                Flex_Bi_L = m / 2500.0f;
            }
            else if (ii>6000 && ii<=7000)
            {
                //chillen
            }
            else if (ii>7000 && ii<9500)
            {
                l = l + emg2.read();
            }
            else if (ii == 9500)
            {
                Flex_Tri_R = l / 2500.0f;
            }
            else if (ii>9500 && ii <=10500)
            {
                //chillen
            }
            else if (ii>10500 && ii<13000)
            {
                k = k + emg3.read();
            }
            else if (ii == 13000)
            {
                Flex_Tri_L = k / 2500.0f;
            }          
    }
}

void OFF()
{
    PwmPin1 = PwmPin2 = 0;
}

void Printing()
{
    float v1 = PwmPin1 * maxVelocity;
    float v2 = PwmPin2 * maxVelocity;

    pc.printf("q1 = %f [rad] \n q2 = %f [rad] \n q1dot = %f [rad/s] \n q2dot = %f [rad/s] \n\n\n\n\n", rad_m1, rad_m2, v1, v2);
}

void StateMachine() 
{
    switch (Active_State)
    {
        case Starting:   
        OFF();            
            for (int i = 0; i<=20; i++)
            {
                led = !led;
                wait(0.05);
                
                if (i == 0)
                {
                    pc.printf("Starting up..\n");
                }    
                else if (i == 20)
                {    
                    Active_State = Calibration;
                    pc.printf("Entering Calibration state \n");
                }
            }                           
        break;
        
        case Calibration:
                //calibration actions
                //pc.printf("Calibration State");
                
                Calibrating();
                OFF();
                if (Knop1==false)
                {
                    pc.printf("Entering Homing state \n");
                    Active_State = Homing;
                }
                
                sample();
                EMG_Read();
                Encoding();    
        break;
        
        case Homing:
                //Homing actions
                //pc.printf("Homing State");
                if (Knop2==false)
                {
                    pc.printf("Entering Funtioning State \n");
                    Active_State = Function;
                }
                
                sample();
                EMG_Read();
                Encoding();
        break;
        
        case Function:
            //pc.printf("Funtioning State");
                
                if (Knop3==false)
                { 
                    pc.printf("Re-entering Calibration State \n");
                    Active_State = Calibration;
                }
               
                sample();
                EMG_Read();
                Encoding();
                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 
    
    StateTicker.attach(&StateMachine, 0.002);
   
    printTicker.attach(&Printing, 4.0);
    
    while(true)
    {
    }
}