File content as of revision 0:545f4726720c:

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

PwmOut pwmpin(D5);
DigitalOut directionpin(D4);
AnalogIn potmeter(A2);

AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );

MODSERIAL pc(USBTX, USBRX);
QEI wheel (D13, D12, NC, 32);

float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 

float mean(float *samples, int n) {
    float sum = 0.0;
    for (int i=0; i<n; i++) {
        sum += samples[i];
    }
    return sum / (float)n;
}

int main()
{   
    pc.baud(115200);
    
    pwmpin.period_us(60);
    int pulses = 0;
    float angle;
    int n = 1;
    int i = 0;
    float meanmuscle;
    float u[500];
    float dxmax = 0.1;
    
    while (true) {
        u[i] = emg0.read();
        
        pulses = wheel.getPulses();
        angle = pulses*angle_resolution;      
        if(n%500 == 0){
            pc.printf("Angle is: %f degrees \r\n", meanmuscle);
            
            // center signal around 0
            u = u - mean(u);
            // here we have to filter the signal and take absolute value
            //
            // now take mean of filtered signal and convert to a dx
            y = mean(u);
            if(y < 0.02){
                dx = 0;
            }
            else if(y > 0.2){
                dx = 1
            }
            else{
                dx=(y - 0.02)/0.18*dxmax;
            }
            // 0.02 and 0.2 obtained from analysing filtered output
            // use dx for inverse kinematics combined with current x to obtain
            // desired positiion. this will be thetaref     
            meanmuscle = (mean(u,500)-0.1)*4;
            directionpin = meanmuscle > 0.0f;
            pwmpin = fabs(meanmuscle);
            i=0;
        }
        n++;    
        i++;
        wait(0.001);     
    }
}