File content as of revision 10:39ec51206c8b:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>

AnalogIn emg0_in            (A0);
AnalogIn emg1_in            (A1);
AnalogIn emg2_in            (A2);

DigitalIn button2           (D10);                  //Let op, is deze niet bezet?
InterruptIn encoderA        (D9);
InterruptIn encoderB        (D8);

DigitalOut directionpin1    (D4);
DigitalOut directionpin2    (D7);
PwmOut pwmpin1              (D5);
PwmOut pwmpin2              (D6);


MODSERIAL pc(USBTX, USBRX);


//Global variables
int encoder = 0;

//Biquad
BiQuadChain emg0band;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );

BiQuadChain emg1band;
BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );

BiQuadChain emg2band;
BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );

BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter


//Tickers
Ticker emg_filter_tick;
Ticker Mov_av_tick;

//Functions
void EMGFilter0()
{
    double emg0         = emg0_in.read();
    double bandpass     = emg0band.step(emg0);
    double absolute     = fabs(bandpass);
    double notch0       = notch1.step(absolute);
}

void EMGFilter1()
{
    double emg1         = emg1_in.read();
    double bandpass     = emg1band.step(emg1);
    double absolute     = fabs(bandpass);
    double notch1       = notch1.step(absolute);
}

void EMGFilter2()
{
    double emg2         = emg2_in.read();
    double bandpass     = emg2band.step(emg2);
    double absolute     = fabs(bandpass);
    double notch2       = notch1.step(absolute);
}

void encoderA_rise()       
{
    if(encoderB==false)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderA_fall()      
{
    if(encoderB==true)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderB_rise()       
{
    if(encoderA==true)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderB_fall()      
{
    if(encoderA==false)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}


// Main function start.

int main()
{ 
    pc.baud(115200);
    pc.printf("hello\n\r");
    
    //EMG signaal filteren 
        
        // Deze? of die hierondbqc.add( &bq1 ).add( &bq2 ).add( &bq3 );
        bqc = bq1 * bq2 * bq3;
        emgSampleTicker.attach(&emgSample,0.01); //100 Hz
        while(true){/*do not return from main()*/}
        
        
    pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 

    encoderA.rise(&encoderA_rise);
    encoderA.fall(&encoderA_fall);
    encoderB.rise(&encoderB_rise);
    encoderB.fall(&encoderB_fall);
    
    while (true)
    {
        //Motor aansturen en encoder uitlezen
          float u1 = potmetervalue1;
          float u2 = potmetervalue2;
          
          float m1 = ((u1*2.0f)-1.0f);
          float m2 = ((u2*2.0f)-1.0f);
        
        pwmpin1 = fabs(m1*0.6f)+0.4f;     //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.        
        directionpin1.write(m1>0);        //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. 
        wait(0.01f);                   //zodat de code niet oneindig doorgaat.
        pwmpin2 = fabs(m2*0.6f)+0.4f;    
        directionpin2.write(m2>0);   
        
        float encoderDegrees = float(encoder)*(360.0/8400.0);
                
        pc.printf("Encoder count: %f \n\r",encoderDegrees);
        
    }
}