File content as of revision 37:af85a7b57a25:

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


//Define objects
//Define the button interrupt for the calibration
InterruptIn button_calibrate(PTA4);

//Define the EMG inputs
AnalogIn    emg1( A0 );
AnalogIn    emg2( A1 );
AnalogIn    emg3( A2 );

//Define the Tickers
Ticker      pos_timer;
Ticker      sample_timer;

HIDScope    scope( 6 );
MODSERIAL pc(USBTX, USBRX);

//Initialize all variables
volatile bool sampletimer = false;
volatile bool buttonflag = false;
volatile bool newcase = false;

double threshold = 0.04;
double samplefreq=0.002;
double emg02;
double emg12;
double emg22;
double ref_x=0.0000;
double ref_y=0.0000;
double old_ref_x;
double old_ref_y;
double speed_emg=0.00002;
double speed_key=0.000326;
double speed=0.00002;
double theta=0.0;
double radius=0.0;

const double minRadius=0.3;                // minimum radius of arm
const double maxRadius=0.6;                // maximum radius of arm
const double minAngle=-1.25;               // minimum angle for limiting controller

char key;


// create a variable called 'mystate', define it
typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states;
states mystate = STATE_PAUZE;

//Define the needed Biquad chains
BiQuadChain bqc11;
BiQuadChain bqc13;
BiQuadChain bqc21;
BiQuadChain bqc23;
BiQuadChain bqc31;
BiQuadChain bqc33;

//Define the BiQuads for the filter of the first emg signal
//Notch filter
BiQuad bq111(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
BiQuad bq112(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
BiQuad bq113(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
//High pass filter
//BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
BiQuad bq121( 0.8956,   -1.7911,    0.8956,    1.0000,   -1.7814,    0.7941);
BiQuad bq122( 0.9192,   -1.8385,    0.9192,    1.0000,   -1.8319,    0.8450);
BiQuad bq123( 0.9649,   -1.9298,    0.9649,    1.0000,   -1.9266,    0.9403);
//Low pass filter
BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );

//Define the BiQuads for the filter of the second emg signal
//Notch filter
BiQuad bq211 = bq111;
BiQuad bq212 = bq112;
BiQuad bq213 = bq113;
/*  High pass filter*/
BiQuad bq221 = bq121;
BiQuad bq222 = bq122;
BiQuad bq223 = bq123;
/*  Low pass filter*/
BiQuad bq231 = bq131;

//Define the BiQuads for the filter of the third emg signal
//notch filter
BiQuad bq311 = bq111;
BiQuad bq312 = bq112;
BiQuad bq313 = bq113;
//High pass filter
BiQuad bq321 = bq121;
BiQuad bq323 = bq122;
BiQuad bq322 = bq123;
//low pass filter
BiQuad bq331 = bq131;

void sampleflag()
{
     if (sampletimer==true) {
        pc.printf("rate too high error in setgoflag\n\r");
    }
    //This sets the go flag for when the function sample needs to be called
    sampletimer=true;
}

void calibrate()
{
    //This resets the reference signals so that the robot can be calibrated
    ref_x=0.0000;
    ref_y=0.0000;
}

void sample(states &mystate)
{
    states myoldstate=mystate;

    //This checks if a key is pressed and adjusts the speed to the needed speed
    if (pc.readable()==1) {
        
        key=pc.getc();
        //printf("%c\n\r",key);
    } 


    //Read the emg signals and filter it

    scope.set(0, emg1.read());                          //original signal 0
    emg02=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 0
    scope.set(1, emg02);
    scope.set(2, emg2.read());                          //original signal 1
    emg12=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 1
    scope.set(3, emg12);
    scope.set(4, emg3.read());                          //original signal 2
    emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
    scope.set(5, emg22);

emg02=1;
    //Ensure that enough channels are available at the top (HIDScope scope( 6 ))
    //Finally, send all channels to the PC at once
    scope.send();

    old_ref_x=ref_x;
    old_ref_y=ref_y;
    //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference.
    if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
        mystate = STATE_XY_NEG;
        ref_x=ref_x-speed;
        ref_y=ref_y-speed;

    } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
        mystate = STATE_X_NEG;
        ref_x=ref_x-speed;

    } else if (emg02>threshold&&emg22>threshold || key == 's') {
        mystate = STATE_Y_NEG;
        ref_y=ref_y-speed;

    } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
        mystate = STATE_XY;
        ref_x=ref_x+speed;
        ref_y=ref_y+speed;

    } else if (emg12>threshold || key == 'q' ) {
        mystate = STATE_X;
        ref_x=ref_x+speed;

    } else if (emg22>threshold || key == 'w') {
        mystate = STATE_Y;
        ref_y=ref_y+speed;
    } else {
        mystate = STATE_PAUZE;
    }

    // convert ref to gearbox angle
    theta=atan((ref_y+sin(theta)*minRadius)/(ref_x+cos(theta)*minRadius));
    radius=sqrt(pow(ref_x+cos(theta)*minRadius,2)+pow(ref_y+sin(theta)*minRadius,2));
    if (theta != theta) {
        theta=0;
    }
    if (theta <= minAngle) {
        ref_x=old_ref_x;
        ref_y=old_ref_y;
        pc.printf("fout 1 ");
    } else if (radius < minRadius) {
        ref_x=old_ref_x;
        ref_y=old_ref_y;
        pc.printf("fout 2 ");
    } /*else if (theta >= 0 ) {
        ref_x=old_ref_x;
        ref_y=old_ref_y;
        pc.printf("fout 3 ");
    }*/ else if ( radius > maxRadius) {
        ref_x=old_ref_x;
        ref_y=old_ref_y;
        pc.printf("fout 4 ");
    }

    //change newcase so that the state will only be printed once
    if (myoldstate==mystate) {
        newcase=false;

    } else {
        newcase=true;
    }
}

void my_pos()
{
    //This function is attached to a ticker so that the reference position is printed every second.
    pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);

}

void print_state()
{
    //This code looks in which state the robot is in and prints it to the screen
    if (newcase==true) {
        switch (mystate) {
            case STATE_CALIBRATION : { // calibration
                pc.printf("calibration\n\r");
                break;
            }
            case STATE_X : // X direction
                pc.printf("X\n\r");
                break;
            case STATE_X_NEG : // negative X direction
                pc.printf("Xneg\n\r");
                break;
            case STATE_Y : // Y direction
                pc.printf("Y\n\r");
                break;
            case STATE_Y_NEG : // negative Y direction
                pc.printf("Yneg\n\r");
                break;
            case STATE_XY : // X&Y direction
                pc.printf("XY\n\r");
                break;
            case STATE_XY_NEG : // negative X&Y direction
                pc.printf("XYneg\n\r");
                break;
            case STATE_PAUZE : // Pauze: do nothing
                pc.printf("PAUZE\n\r");
                break;
        }
    }
}

int main()
{
    pc.printf("RESET\n\r");
    pc.baud(115200);

    //Initialize the Biquad chains
    bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
    bqc13.add( &bq131);
    bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
    bqc23.add( &bq231);
    bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
    bqc33.add( &bq331);

    //Attach the 'sample' function to the timer 'sample_timer'.
    //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
    sample_timer.attach(&sampleflag, samplefreq);
    //Attach the function calibrate to the button interrupt
    button_calibrate.fall(&calibrate);
    //Attach the function my_pos to the timer pos_timer.
    //This ensures that the position is printed every second.
    pos_timer.attach(&my_pos, 1);

    while(1) {
        //Only take samples when the go flag is true.
        if (sampletimer==true) {
            sample(mystate);
            print_state();
            sampletimer = false;
        }
    }
}