File content as of revision 14:dc89250ebc52:

#include "mbed.h"
#include "HIDScope.h"
//#include "QEI.h"
#include "MODSERIAL.h"
//#include "BiQuad.h"
#include "FastPWM.h"
#include <iostream>
MODSERIAL pc(USBTX, USBRX);
AnalogIn ain2(A2);
AnalogIn ain1(A3);
DigitalOut dir2(D4);
DigitalOut dir1(D7);



Ticker ticktick;
//D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
PwmOut motor2_pwm(D5);
PwmOut motor1_pwm(D6);
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );

Ticker      sample_timer;
HIDScope    scope( 2 );
DigitalOut  ledred(LED_RED);
DigitalOut  ledblue(LED_BLUE);
DigitalOut  ledgreen(LED_GREEN);
volatile float P;
enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
states currentState=Operating;
InterruptIn err(SW2);


void read_emg()
{
    static int count=0;
    static float RMS_value=0;
    static float HighPass_value=0;
    count+=1;
    static float RMS[150];
    static float HighPass[30];
    float I1;
    float If;
    I1=emg0.read(); //read signal
    HighPass_value+=(I1-HighPass[count%30])/30.0;
    HighPass[count%30]=I1;
    If=pow(I1-HighPass_value,2.0f); // Highpass-filtered value squared
    RMS_value+=(If-RMS[count%150])/150.0;
    RMS[count%150]=If;
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    P=sqrt(RMS_value);
    scope.set(0, P ); // send root mean squared
    scope.set(1, emg0.read() );
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */
    scope.send();
    /* To indicate that the function is working, the LED is toggled */
    ledred=1;
    ledgreen=0;
    ledblue=1;
}

void set_PWM(void)
{
   float Q;
   if (7*P>1)
   {
       Q=1.0;
    }
    else
    {
       Q=7*P;
    }
   motor1_pwm.write(Q);
   motor2_pwm.write(ain1.read());
}

void sample()
{
    switch(currentState)
    {
        case Operating:
            read_emg();
            set_PWM();
            break;
        case Failure:
            ledred=0;
            ledgreen=1;
            ledblue=1;
            break;
        default:
            ledred=1;
            ledgreen=1;
            ledblue=0;
        break;
    }
}

void error_occur()
{
    currentState=Failure;
}

int main()
{
    pc.baud(115200);
    pc.printf("Starting...");
    ledred=0;
    sample_timer.attach(&sample, 0.002);
    err.fall(error_occur);
    int frequency_pwm=10000;
    motor1_pwm.period(1.0/frequency_pwm);
    motor2_pwm.period(1.0/frequency_pwm);
    
    while (true) {
        wait(10);
        dir1=!dir1;
        dir2=!dir2;
    }
}