File content as of revision 22:88203d281e98:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "FilterDesign.h"
#include "BiQuad.h"
#include "BiQuad4.h"
#include "HIDScope.h"

QEI Encoder(D12,D13,NC,32);

DigitalOut M1(D4);
DigitalOut M2(D7);


PwmOut E1(D5);
PwmOut E2(D6);


AnalogIn    emg0( A0 ); // EMG input 1
AnalogIn    emg1( A1 );  // EMG input 2

float potVal1;
float potVal2;
volatile double emg1_filtered;      //measured value of the first emg
volatile double emg2_filtered;      //measured value of the second emg
Ticker      sample_timer;           // Ticker for reading out EMG
HIDScope    scope( 2 );

float pi = 3.14159265359;

/** Sample function
 * this function samples the emg and sends it to HIDScope
 **/
 
void sample()
{
    emg1_filtered = emg0.read() ;
    emg2_filtered = emg1.read() ;
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    scope.set(0, emg1_filtered);
    scope.set(1, emg2_filtered);
    /* 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 */
    //led = !led;
}

void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
{
    float pErrorC;
    float pErrorP = 0;
    float iError = 0;
    float dError;

    float Kp = 5;
    float Ki = 0.01;
    float Kd = 0.7;

    float rotC = Enc->getPulses()/(32*131.25);
    float rotP = 0;
    float MotorPWM;

    Timer t;
    float tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        pErrorC = rotDes - rotC;
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;

        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }

        rotP = rotC;
        rotC = Enc->getPulses()/(32*131.25);
        tieme = t.read();
        t.reset();
        //pc.printf("pError: %f dError: %f iError: %f PWM: %f\n\r", pErrorC, dError, iError, MotorPWM);
    } while (pErrorC > 0.01 || pErrorC < -0.01 ||dError > 0.01 || dError < -0.01);
    //*E = 0;
    t.stop();
}



int main()
{   
    /**Attach the 'sample' function to the timer 'sample_timer'.
    * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
    */
    sample_timer.attach(&sample, 0.002);
    float steps = 100;

    /*empty loop, sample() is executed periodically*/
    while (true){
        if(emg1_filtered >0.6) {
        for(int i = 0; i < steps; i++){
            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
            //pc.printf("step: %f\n\r", float(i)/steps);
        }
        for(int i = steps; i > 0; i--){
            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
        }            
    }
    }
}