File content as of revision 17:7dd6dc3c7902:

#include "mbed.h"
#include "MODSERIAL.h"
AnalogIn emgtr(PTB3);
AnalogIn emgbr(PTB2);
//Rechts is x, links is y
AnalogIn emgtl(PTB1);
AnalogIn emgbl(PTB0);
PwmOut pwm_x(PTA12); //Motor A
PwmOut pwm_y(PTA5);//Motor B
MODSERIAL pc(USBTX,USBRX);
DigitalOut motordirA(PTD3);
DigitalOut motordirB(PTD1);

volatile bool looptimerflag;
void setlooptimerflag(void)
{
looptimerflag = true;
}

volatile bool dirflagx=true;
volatile bool dirflagy=true;

void tricheck(void)
{
dirflagx=true;
dirflagy=true;
}

int main()
{
pwm_x.period(1.0/22000.0);
pwm_y.period(1.0/22000.0);
    Ticker looptimer;
    Timeout dirtimeout;
    const double ts=0.001;
    looptimer.attach(setlooptimerflag,ts);
    double numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
    double xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
    double xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
    double xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
    double xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
    double zx,zy;
    int xdir;
    int ydir;

    x1tr=0;    y1tr=0;    z1tr=0;    z2tr=0;    yabs1tr=0;    yabs2tr=0;
    x1br=0;    y1br=0;    z1br=0;    z2br=0;    yabs1br=0;    yabs2br=0;
    x1tl=0;    y1tl=0;    z1tl=0;    z2tl=0;    yabs1tl=0;    yabs2tl=0;
    x1bl=0;    y1bl=0;    z1bl=0;    z2bl=0;    yabs1bl=0;    yabs2bl=0;
    zx=0; zy=0;
    xdir=0; ydir=0;

    //High pass, 35Hz, 1e
    numh1=0.900575535279376;
    numh2=-0.900575535279376;
    //denh1=1;
    denh2=-0.801151070558751;

    //Low pass, 2 Hz, 2e orde
    //numl1=0.391302053991682*pow(10.0,-4.0);
    //numl2=0.782604107983365*pow(10.0,-4.0);
    //numl3=0.391302053991682*pow(10.0,-4.0);
    numl1=0.000944691843840;
    numl2=0.001889383687680;
    numl3=0.000944691843840;
    //denl1=1;
    //denl2=-1.982228929792529;
    //denl3=0.982385450614126;
    denl2=-1.911197067426073;
    denl3=0.914975834801434;
    pc.baud(115200);

    while(1) {
        while(looptimerflag != true);{
        }
        looptimerflag = false;

ktr=emgtr.read();
        xtr=(ktr-0.5)*2.0;
        ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
        yabstr=abs(ytr);
        ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
        x1tr=xtr;        y1tr=ytr;        z2tr=z1tr;        z1tr=ztr;        yabs2tr=yabs1tr;        yabs1tr=yabstr;

kbr=emgbr.read();
        xbr=(kbr-0.5)*2.0;
        ybr=xbr*numh1+x1br*numh2-y1br*denh2;
        yabsbr=abs(ybr);
        zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
        x1br=xbr;        y1br=ybr;        z2br=z1br;        z1br=zbr;        yabs2br=yabs1br;        yabs1br=yabsbr;

ktl=emgtl.read();
        xtl=(ktl-0.5)*2.0;
        ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
        yabstl=abs(ytl);
        ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
        x1tl=xtl;        y1tl=ytl;        z2tl=z1tl;        z1tl=ztl;        yabs2tl=yabs1tl;        yabs1tl=yabstl;

kbl=emgbl.read();
        xbl=(kbl-0.5)*2.0;
        ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
        yabsbl=abs(ybl);
        zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
        x1bl=xbl;        y1bl=ybl;        z2bl=z1bl;        z1bl=zbl;        yabs2bl=yabs1bl;        yabs1bl=yabsbl;
        
        zx=(zbr*4.0);
        zy=(zbl*4.0);
        
        if (zx>1.0) {
            zx=0.99999;
        }
        if (zy>1.0) {
            zy=0.99999;
        }
        
        if (zx<0.20){
        zx=0;
        }
        if (zy<0.20){
        zy=0;
        }
        
        if ((ztr>(zbr+0.1)) && dirflagx == true) {
            dirflagx = false;
            xdir ^= 1;
            zx=0;
            dirtimeout.attach(tricheck,1.5);
        }
          if ((ztl>(zbl+0.1)) && dirflagy == true) {
            dirflagy = false;
            ydir ^= 1;
            zy=0;
            dirtimeout.attach(tricheck,1.5);
        }
        motordirA.write(xdir);
        motordirB.write(ydir);
        
        pwm_x.write(abs(zx));
        pwm_y.write(abs(zy));
        pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
    }
}