File content as of revision 8:364ea64ae86b:

#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 <HIDScope.h>


//#include  <MODSERIAL.h>

// hidscope
HIDScope    scope( 4 );
bool startCalc;
bool calpos1;
bool calpos2;
bool bas;
int waiting;
int count1;
int count2;
int count3;
int count4;
InterruptIn button(SW3);

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

volatile float x1;
volatile float y1;
double emgFiltered3;
double emgFiltered23;
bool dir = true;
// filtering
//filter coeffiecents
// highpass
double b01h = 0.956543225556877;
double b02h = -1.91308645111375;
double b03h =  0.956543225556877;
double a01h = -1.91119706742607;
double a02h =   0.914975834801434;
// notchfilter
double b01n = 0.991103635646810;
double b02n =   -1.60363936885013;
double b03n = 0.991103635646810;
double a01n =   -1.60363936885013;
double a02n =  0.982207271293620;
//lowpass 1
double b01l =  0.000346041337639103;
double b02l =  0.000692082675278205;
double b03l =  0.000346041337639103;
double a01l =  -1.94669754075618;
double a02l =  0.948081706106740;
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);
DigitalOut Led2(LED2);
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;
int i;

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 = -40; // current position
double ey = -30; // 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) {
        Led2 = 0;
        i = 0;
        Cxx =x1;
        if (dir == true) {
            if(x1 > -46.3) {
                x1 = x1-4.2;
                //return x1;

            } else if ( x1 <= -46.3) {
                x1 =-17;
                //return x1;
            } else {
            }
        } else {
            if(x1 < -17) {
                x1 = x1+4.2;
                //return x1;

            } else if ( x1 >= -17) {
                x1 = -46.3;
                //return x1;
            } else {
            }
        }
    }
}

void Position1y(double b)
{
    if (b > 0.18) {
        Led2 = 0;
        i = 0;
        Cyy=y1;
        if(dir == true) {
            if(y1 < 32.4) {
                y1 = y1+4.2;
                //return y1;
            } else if ( y1 >= 32.4) {
                y1 = 3;
                //return y1;
            } else {
            }
        } else {
            if(y1 > 3) {
                y1 = y1-4.2;
                //return y1;
            } else if ( y1 <= 3) {
                y1 = 32.4;
                //return y1;
            } else {
            }
        }
    }
}
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.008;
            }
            if (ey < Cyy) {
                ey = ey + 0.008;
            }
        }
    } else {
        if (ex > Cxx) {
            ex = ex - 0.008;
        }
        if (ex < Cxx) {
            ex = ex + 0.008;
        }
    }
}







void position_controll_void()
{
    bas = true;
}


void motor_controler()
{
    bb = -(Enc1.getPulses()) - 816;
    bc = Enc2.getPulses() + 4316;

    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 <=2)
        if (bas == true) {
            if (waiting == 1) {
                Cxx = -17;
                Cyy = 3;

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

            }

            if(waiting == 2) {
                Cxx = -45.8;
                Cyy = 7;
                position_define();
                angle_define();
                motor_controler();
            }
        }
}

void setCalibration()
{
    if (startCalc == false) {

        if (calpos1 == false) {
            while(abs(count2-count1) > 0) {
                PMW1.write(0.1f);
                wait(0.1);
                PMW1.write(0);
                count2 = count1;
                count1 = Enc1.getPulses();
            }
            Enc1.reset();
            bb= Enc1.getPulses();
            calpos1 = true;
        }
        if(calpos2 == false) {
            while(abs(count4-count3) > 0) {
                PMW2.write(0.1f);
                M1=0;
                PMW1.write(0.1f);
                wait(0.1);
                PMW2.write(0);
                PMW1.write(0);
                count4 = count3;
                count3 = Enc2.getPulses();
            }
            Enc2.reset();
            bc= Enc2.getPulses();
            calpos2 = true;
        }
    }
}


int main()
{
    Led = 1;
    Led2 = 0;
    M1 = 1;
    M2 = 1;
    startCalc = false;
    calpos1 = false;
    calpos2 = false;
    count2 = 10000;
    count4 = 10000;
    Led = 1;
    PMW1.write(0.1f);
    wait(0.1);
    PMW1.write(0);
    count1 = Enc1.getPulses();
    PMW2.write(0.1f);
    wait(0.1);
    PMW2.write(0);
    count3 = Enc2.getPulses();
    
    setCalibration();
    Led2 = 1;
    button.fall(&change_wait);
    PMW1.period_us(60);
    PMW2.period_us(60);
    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 );
    Cxx = -35;
    Cyy = 27;
    i = 251;
    while(true) {

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

            scope.set(0, ex); // filtered 1
            scope.set(1, Cxx); //filtered signal 2
            scope.set(2, ey);
            scope.set(3, Cyy);
            scope.send();

            if (i <= 250) {
                emgFiltered3 = 0;
                emgFiltered23 = 0;
                i++;
            }


            sw2.fall(change);
            Position1x(emgFiltered3);
            Position1y(emgFiltered23);
            Cxx = x1;
            Cyy = y1;
            Led2 = 1;
            Led = 0;
            bas= false;
        }
    }

}