File content as of revision 3:40427c0157a0:

#include "mbed.h"
#include "BiQuad.h"
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <ctime>
#include <QEI.h>
#include "PID_controler.h"
//#include  <MODSERIAL.h>

bool bas;
int waiting;
InterruptIn button(SW3);

Serial pc(USBTX, USBRX);
// emg signals input
AnalogIn   emg1(A0);
AnalogIn   emg2(A1);
InterruptIn sw2(SW2);
// tickers
Ticker sample_timer;

volatile int x1;
volatile int y1;
double emgFiltered3;
double emgFiltered23;
bool dir = true;
// filtering
//filter coeffiecents
// highpass
double b01h = 0.978030479206560;
double b02h = -1.95606095841312;
double b03h =  0.978030479206560;
double a01h = -1.95557824031504;
double a02h =   0.956543676511203;
// notchfilter
double b01n = 0.995532032687234;
double b02n =   -1.89361445373551;
double b03n = 0.995532032687234;
double a01n =   -1.89361445373551;
double a02n =  0.991064065374468 ;
//lowpass 1
double b01l =  8.76555487540065e-05;
double b02l =  0.000175311097508013;
double b03l =  8.76555487540065e-05;
double a01l =  -1.97334424978130;
double a02l =  0.973694871976315;

BiQuadChain bqc;
BiQuad bq1( b01h, b02h, b03h, a01h, a02h ); //highpass
BiQuad bq2( b01n, b02n, b03n, a01n, a02n ); //notch
// than we need to rectifie
// and lowpass afterwards
BiQuadChain bqc2;
BiQuad bq3( b01l, b02l, b03l, a01l, a02l);  //lowpass
// optional is doing a movingaverage after

BiQuadChain bqc3;
BiQuad bq4( b01h, b02h, b03h, a01h, a02h ); //highpass
BiQuad bq5( b01n, b02n, b03n, a01n, a02n ); //notch
// than we need to rectifie
// and lowpass afterwards
BiQuadChain bqc4;
BiQuad bq6( b01l, b02l, b03l, a01l, a02l);  //lowpass


//Define in/outputs

int counts = 8400;
DigitalOut Led(LED1);
PwmOut PMW1(D5); // Motor 1
DigitalOut M1(D4); // direction of motor 1
PwmOut PMW2(D6); // Motor 2
DigitalOut M2(D7); // direction of motor 2

//initializing Encoders
QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated

double Kp = 1;
double Ki = 1;
double Kd = 0.3;
double Ts = 0.001;

float counts_a;
float counts_b;


//Define Variables

double pi = 3.14159265359;
int bb;
int bc;
float z;

double angle_a = 0; //in rad
double angle_b = 0.5 * pi;  //in rad

double X0[2][1] = {{angle_a},{angle_b}};
double X[2][1];
double Xold[2][1];
double fval[2][1];
double J[2][2];
double err[2][1];

double MaxIter = 20;
double tolX = 1e-4;
double A = 20;
double B = 30;
double C = 20;
double D = 27;
double E = 35;
double ex = -35; // current position
double ey = 27; // current position
double Cxx = -35; // Goal position
double Cyy = 27; // Goal position

Ticker position_controll;

void filteren ()
{
    double emgSignal1 = emg1.read();
    double emgSignal2 = emg2.read();

    double emgFiltered1 = bqc.step(emgSignal1);
    double emgFiltered2 = fabs(emgFiltered1);
    emgFiltered3 = bqc2.step(emgFiltered2);

    double emgFiltered21 =  bqc3.step(emgSignal2);
    double emgFiltered22 = fabs(emgFiltered21);
    emgFiltered23 = bqc4.step(emgFiltered22);

}

void Position1x(double b)
{
    if (b > 0.20) {
        Cxx =x1;
        if (dir == true) {
            if(x1 > -46) {
                x1 = x1-4;
                pc.printf(" posx is %f\n\r",Cxx);
                //return x1;

            } else if ( x1 <= -46) {
                x1 =-14;
                //return x1;
            } else {
            }
        } else {
            if(x1 < -14) {
                x1 = x1+4;
                //return x1;

            } else if ( x1 >= -14) {
                x1 = -46;
                //return x1;
            } else {
            }
        }

        wait(0.5);
    }
}

void Position1y(double b)
{
    if (b > 0.20) {
        Cyy=y1;
        if(dir == true) {
            if(y1 < 43) {
                y1 = y1+4;
                //return y1;
            } else if ( y1 >= 43) {
                y1 = 11;
                //return y1;
            } else {
            }
        } else {
            if(y1 > 11) {
                y1 = y1-4;
                //return y1;
            } else if ( y1 <= 11) {
                y1 = 43;
                //return y1;
            } else {
            }
        }

        wait(0.5);
    }
}
void change()
{
    dir = !dir;
}
void NR() //Newton Rapshon Calculation
{
    //Variables
    double Hoa = X[0][0];
    double Hob = X[1][0];

    double meuk1 = cos(Hoa) * A - ((E + C)/E) * (cos(Hob)*D - ex) - ex;
    double meuk2 = sin(Hoa) * A - ((E + C)/E) * (sin(Hob)*D - ey) - ey;

    //Define f(x)
    fval[0][0] = pow((ex - D * cos(Hob)),2) + pow((ey - D * sin(Hob)),2) - pow((E),2);
    fval[1][0] = pow((meuk1),2) + pow((meuk2),2) - pow((B),2);
    //Jacobian



    J[0][0]= 0;
    J[0][1]= 2 * D * sin(Hob) * (ex - D * cos(Hob)) - 2 * D * cos(Hob) * (ey - D * sin(Hob));
    J[1][0]=  - 2 * A * sin(Hoa) * meuk1 + 2 * A * cos(Hoa)* meuk2;
    J[1][1]= 2 * ((E + C)/E) * D * sin(Hob) * meuk1 - 2 * ((E + C)/E) * D * cos(Hob) * meuk2;
}

void angle_define() //define the angle needed.
{
    for(int i=1 ; i <= MaxIter; i++) {
        NR();

        X[0][0] = X[0][0] - ((-J[1][1]/(J[0][1] *J[1][0]))*fval[0][0] + (1/J[1][0])* fval[0][1]);
        X[1][0] = X[1][0] - ((1/J[0][1])*fval[0][0]);

        err[0][0] = abs(X[0][0] - Xold[0][0]);
        err[1][0] = abs(X[1][0] - Xold[1][0]);

        Xold[0][0] = X[0][0];
        Xold[1][0] = X[1][0];

        counts_a = ((X[0][0]) / (2* pi)) * 8400;
        counts_b = ((X[1][0]) / (2* pi)) * 8400;

        if(err[0][0] <= tolX) {
            if(err[1][0] <= tolX) {
                break;
            }
        }
    }
}

void position_define()
{
    if (ex >= Cxx - 0.01 && ex <= Cxx + 0.01) {
        if (ey >= Cyy - 0.01 && ey <= Cyy + 0.01) {
        } else {
            if (ey > Cyy) {
                ey = ey - 0.004;
            }
            if (ey < Cyy) {
                ey = ey + 0.004;
            }
        }
    } else {
        if (ex > Cxx) {
            ex = ex - 0.004;
        }
        if (ex < Cxx) {
            ex = ex + 0.004;
        }
    }
}







void position_controll_void()
{
    bas = true;
}


void motor_controler()
{
    bb = -(Enc1.getPulses()) - 201;
    bc = Enc2.getPulses() + 2100;

    if (bb >= counts_a) {
        z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
        PMW1.write(abs(z));
        M1 = 1;
    }
    if (bb <= counts_a) {
        z = PID_controller((counts_a - bb),Kp, Ki, Kd, Ts);
        PMW1.write(abs(z));
        M1 = 0;
    }
    if (bc >= counts_b) {
        M2 = 0;
        z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
        PMW2.write(abs(z));
    }
    if (bc <= counts_b) {
        M2 = 1;
        z = PID_controller((counts_b - bc),Kp, Ki, Kd, Ts);
        PMW2.write(abs(z));
    }
}


void change_wait()
{
    waiting = waiting++;
}


void initializeren()
{
    waiting = 1;
    while(waiting <=3)
    if (bas == true)
    {
        if (waiting == 1) {
            Cxx = -20;
            Cyy = 10;

            position_define();
            angle_define();
            motor_controler();

        }

    if(waiting == 2) {
        Cxx = -40;
        Cyy = 10;
        position_define();
        angle_define();
        motor_controler();
        button.fall(&change_wait);
    }
    }
}


int main()
{
    Led = 1;
    PMW1.period_us(60);
    button.fall(&change_wait);
    position_controll.attach(position_controll_void,0.002);
    X[0][0] = X0[0][0];
    X[1][0] = X0[1][0];
    Xold[0][0] = X0[0][0];
    Xold[1][0] = X0[1][0];
    //pc.baud(115200);
    initializeren();
    x1 = Cxx;
    y1= Cyy;
    bqc.add( &bq1 ).add( &bq2 );
    bqc2.add( &bq3 );
    bqc3.add( &bq4 ).add( &bq5 );
    bqc4.add( &bq6 );

    while(true) {

        if(bas == true) {
            Led = 1;
            filteren();
            position_define();
            angle_define();
            motor_controler();

            sw2.fall(change);
            Position1x(emgFiltered3);
            Position1y(emgFiltered23);
            Led = 0;
            bas= false;
        }
    }

}