File content as of revision 21:659f7c8ed328:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#define PI 3.14159265358979323
//XenY
//Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks.
//4123 ticks is een rondje.
//Rechts is x, links is y
AnalogIn emgtr(PTB3);
AnalogIn emgbr(PTB2);
AnalogIn emgtl(PTB1);
AnalogIn emgbl(PTB0);
PwmOut pwm_A(PTA12);
PwmOut pwm_B(PTA5);
MODSERIAL pc(USBTX,USBRX);
DigitalOut motordirA(PTD3);
DigitalOut motordirB(PTD1);
Encoder motor1(PTD0,PTC9);
Encoder motor2(PTD5,PTC8);

void keep_in_range(float * in, float min, float max);

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_A.period(1.0/22000.0);
    pwm_B.period(1.0/22000.0);
    Ticker looptimer;
    Timeout dirtimeout;
    const float ts=0.001;
    looptimer.attach(setlooptimerflag,ts);
    float numh1,numh2,denh2,numl1,numl2,numl3,denl2,denl3;
    float xtr,ytr,y1tr,x1tr,ztr,z1tr,z2tr,yabstr,yabs1tr,yabs2tr,ktr;
    float xbr,ybr,y1br,x1br,zbr,z1br,z2br,yabsbr,yabs1br,yabs2br,kbr;
    float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
    float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
    float zx,zy;
    float xuit,yuit, rt;
    //float zx1, zy1, gain;
    //float pwmA, pwmB;
    int xdir, ydir, Adir, Bdir;
    float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
    float for_A, for_B, yuit1, xuit1;
    int ticka, tickb, refA,refB, errA, errB;
    float ctrlA;
    float ctrlB;
    //Startwaarden
    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;
    xuit=0;
    yuit=0;
    kp=0.1;
    ki=0.01;
    kd=0.01;
    rt=0.032805;
    Bi1=0;
    Bd1=0;
    yuit1=0;
    xuit1=0;

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

    //Low pass, 5 Hz, 2e orde.
    numl1=0.241359049041961*pow(10.0,-3.0);
    numl2=0.482718098083923*pow(10.0,-3.0);
    numl3=0.241359049041961*pow(10.0,-3.0);
    //denl1=1;
    denl2=-1.955578240315036;
    denl3=0.956543676511203;
    pc.baud(115200);

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

//EMG lezen.
        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*5.0);
        zy=(zbl*5.0);

        //Grenzen.
        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;
        }

        //Richting omdraaien met triceps.
        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);
        }

        //Motoraansturing.
        if (ydir==1) {
            zy=-1.0*zy;
        }
        if (xdir==1) {
            zx=-1.0*zx;
        }

        ticka=motor1.getPosition();
        tickb=motor2.getPosition();
        vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s
        vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s
        xuit += ts*vxuit;
        yuit += ts*vyuit;
        yuit1=yuit;

        refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI));
        refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
        errA=refA-ticka;
        errB=refB-tickb;

        if(errA<0) {
            Adir=0;
        } else {
            Adir=1;
        }
        if(errB<0) {
            Bdir=0;
        } else {
            Bdir=1;
        }
        //Controllers
        Ap=errA*kp;
        Ad=(errA-Ad1)*kd/ts;
        Ai=(Ai1+ts*errA)*ki;
        keep_in_range(&Ai, -0.1,0.1);
        Ad1=Ad;
        Ai1=Ai;
        ctrlA=Ai+Ap+Ad;
        for_A = ctrlA/1000.;
        
        errB= 1;
        Bp=errB*kp;
        Bd=(errB-Bd1)*kd/ts;
        Bi=(Bi1+ts*errB)*ki;
        keep_in_range(&Bi, -0.1,0.1);
        Bd1=Bd;
        Bi1=Bi;
        ctrlB=Bi+Bp+Bd;
        for_B= ctrlB / 1000.0;
        //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.

        keep_in_range(&for_A, -1,1);
        keep_in_range(&for_B, -1,1);

        motordirA.write(Adir);
        motordirB.write(Bdir);
        pwm_A.write(for_A);
        pwm_B.write(for_B);

        //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
        pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
        //pc.printf("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB);
    }
}

void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}