File content as of revision 37:c0f6e3b44d7b:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "math.h"

//Define objects
AnalogIn    emg( A0 ); //EMG
AnalogIn    emg1( A1 ); //EMG
HIDScope    scope( 2 ); // aantal scopes dat gemaakt kan worden
DigitalOut  ledB(LED_BLUE); 
DigitalOut  ledG(LED_GREEN);
DigitalIn TestButton(PTA4); // button naast het ledje
DigitalIn onoff(PTC6); // button aan de andere kant
Ticker emgSampleTicker; // Ticker voor de sample frequency
PwmOut motor2MagnitudePin(D5); // magnitude motor 2
DigitalOut motor2DirectionPin(D4); // direction of the motor 2
InterruptIn MotorOn(D8);



 int P= 200; // aantal test punten voor de moving average
 double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P
 double B[200];
 int k = 0; // counter voor de configuratie
 double Vvector[200]; // vector voor de Vwaarde configuratie
 double Vwaarde[2]; // vector voor waardes van V
 int x = 0; // x waarde voor de Vwaarde
 double movmeanR;
 double movmeanL; //moving average for the right arm (first emg-shield)
 int MotorLock = 0;
 double EMGInputR;
 double EMGInputL;
 float ErrorZero = 3; 
 double MotorSpeed = 0.5;
 
 
 // Filters 
BiQuadChain bqcR;
BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
BiQuad bq2R( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
BiQuad bq3R(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?
BiQuadChain bqcL;
BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
BiQuad bq2L( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
BiQuad bq3L(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?



// sample function voor plotten van de emg signalen en moving average 
void emgSample() {
    
    double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal
    double emgFilteredL = bqcL.step( emg1.read());
    double emgabsR = abs(emgFilteredR); // absolute waarde van het gefilterde signaal
    double emgabsL = abs(emgFilteredL);
    //scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek
    //scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek
    
    
    // deze for-loop maakt de vector voor de moving average
    for(int i = P-1; i >= 0; i--){
        if (i == 0) {
            A[i] = emgabsR;
            }
         else {
             A[i] = A[i-1];
             }   
    }
    double sumR = 0;
    // deze for-loop sommeert de array
    for (int n = 0; n < P-1; n++) {
    sumR = sumR + A[n];
    }
    
    movmeanR = sumR/P; //dit is de moving average waarde
    
    // deze for-loop maakt de vector voor de moving average
    for(int i = P-1; i >= 0; i--){
        if (i == 0) {
            B[i] = emgabsL;
            }
         else {
             B[i] = B[i-1];
             }   
    }
    double sumL = 0;
    // deze for-loop sommeert de array
    for (int n = 0; n < P-1; n++) {
    sumL = sumL + B[n];
    }
    
    movmeanL = sumL/P; //dit is de moving average waarde
    
    // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
    if (TestButton==0 & k<200) {
        Vvector[k] = movmeanR;
        ledB = !ledB;
        k++;
    }    
    else if (k==200) { // hier moet de test klaar zijn
        double sumZ = 0;
            for (int n = 0; n < 199; n++) {
            sumZ = sumZ + Vvector[n];
            } // neemt de som van de zerovector array
        Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value
        //scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
        if (x == 1) {
          //  scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
        }
        k++;
        ledB = 1;
        ledG = !ledG;
    }
    else if (k == 201 && onoff ==0) {// dit is om het ledje uit te doen en om het mogelijk te maken de test opnieuw te doen
        ledG = !ledG;
        k = 0;
        if (x==0) {
            x++;
        }
        else if (x==1) {
            x=0;
        }
    }
    scope.set(0, movmeanR); // stuurt de moving average naar de plot
    scope.set(1, movmeanL);
    
    if (movmeanR > Vwaarde[1]) {
        movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
    }
    
    double EMGInputRawR =  (movmeanR - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); 
    if (EMGInputRawR < 0) {
        EMGInputR = 0;
    }
    else {
        EMGInputR = EMGInputRawR;
    } 
    //scope.set(5,EMGInputR);  
    
    scope.send();
    
    //motor2MagnitudePin = 0.2;
}

float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    const float maxVelocity=8.4; // in rad/s of course!
    float referenceVelocity;  // in rad/s
    referenceVelocity = (EMGInputR * maxVelocity) * MotorLock; 
    return referenceVelocity;
}

void SetMotor2(float motorValue)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (fabs(motorValue)>1) motor2MagnitudePin = 1;
        else motor2MagnitudePin = fabs(motorValue);
}
float FeedForwardControl(float referenceVelocity) {
    // very simple linear feed-forward control
    const float MotorGain=8.4; // unit: (rad/s) / PWM
    float motorValue = referenceVelocity / MotorGain;
    return motorValue;
}

void MotorLocker() {
    if (MotorLock == 0) {
        MotorLock = 1;
    }
    else if (MotorLock == 1) {
        MotorLock = 0;
    }
}

int main()
{  
ledB = 1;
ledG = 1;
bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // hier wordt het filter gemaakt
bqcL.add( &bq1L ).add( &bq2L ).add( &bq3L ); // hier wordt het filter gemaakt
emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
Vwaarde[0] = 0.01;
motor2DirectionPin = 1;

    while(1) {
        if (Vwaarde[1] == 0 || x==1) {
            MotorLock = 0;
        }
        else MotorLock = 1;
       // MotorOn.rise(&MotorLocker);
        motor2MagnitudePin = EMGInputR*MotorLock;
        wait(0.01);
    }
}