File content as of revision 31:620950f7e514:

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


//Define objects
AnalogIn    emg( A0 ); //EMG
AnalogIn    emg1( A1 ); //EMG
HIDScope    scope( 6 ); // 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
DigitalOut motor2MagnitudePin(D5); // magnitude motor 2
DigitalOut motor2DirectionPin(D4); // direction of the motor 2
InterruptIn MotorOn(D10);


 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
 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 movmean; 
 int MotorLock = 0;
 double EMGInput;
 float ErrorZero = 1.5; 
 
 // Filters 
BiQuadChain bqc;
BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
BiQuad bq2( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
BiQuad bq3(  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 emgFiltered = bqc.step( emg.read() ); // gefilterde waarde van het emg signaal
    double emgabs = abs(emgFiltered); // absolute waarde van het gefilterde signaal
    scope.set(0, emgFiltered ); // stuurt de waarden naar de grafiek
    scope.set(1, emgabs ); // 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] = emgabs;
            }
         else {
             A[i] = A[i-1];
             }   
    }
    double sum = 0;
    // deze for-loop sommeert de array
    for (int n = 0; n < P-1; n++) {
    sum = sum + A[n];
    }
    
    movmean = sum/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] = movmean;
        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;
        }
    }
    double EMGInputRaw =  (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); 
    if (EMGInputRaw < 0) {
        EMGInput = 0;
    }
    else {
        EMGInput = EMGInputRaw;
    } 
    scope.set(5,EMGInput);  
    scope.set(2, movmean); // stuurt de moving average naar de plot
    
    scope.send();
}

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 = (EMGInput * 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 (motorValue>1) motor2MagnitudePin = 1;
        else motor2MagnitudePin = 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;
bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt
emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz

 
    while(1) {
        MotorOn.rise(&MotorLocker);
        motor2DirectionPin = 1;
        SetMotor2(0.5);
            }
}