File content as of revision 38:fb5c45d85ead:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#define PI 3.14159265358979323
//Rechterarm is x, linkerarm is y.

//Scripts voor de Barry Plotter. Gemaakt door Groep 3, BMT K9, 2013-2014.
//Wouter Doppenberg, Gerard Essink, Ralph Lentink, Thomas Plaisier, Linda Zwiers.
//Laatste wijziging: 07-11-13.

//Inputs.
AnalogIn emgtr(PTB3);
AnalogIn emgbr(PTB2);
AnalogIn emgtl(PTB1);
AnalogIn emgbl(PTB0);
PwmOut pwm_A(PTA12);
PwmOut pwm_B(PTA5);
PwmOut redled(LED_RED);
PwmOut greenled(LED_GREEN);
PwmOut blueled(LED_BLUE);
MODSERIAL pc(USBTX,USBRX);
DigitalOut motordirA(PTD3);
DigitalOut motordirB(PTD1);
Encoder motor1(PTD0,PTC9);
Encoder motor2(PTD5,PTC8);

//Functies en flags.
void keep_in_range(double * in, double min, double max);
void keep_in_rangeint(int * in, int min, int max);

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

volatile bool calA=true, calB=true; //Calibratie per motor.
volatile bool frictionflag=true; //Wrijvingscompensatie.

volatile bool calflag=true; //Calibreren?
volatile bool meetflag=true; //Meten?

int main()
{
//Constanten en tickers.
    pwm_A.period(1.0/2500.0);
    pwm_B.period(1.0/2500.0);
    Ticker looptimer;
    Timeout dirtimeout;
    const double ts=0.004;
    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, xuit,yuit, rt;
    double vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
    double for_A, for_B, ctrlA, ctrlB, kdc, kpc, kic;
    double gain, emggrens,frictiona,frictionb, schrijfgainx, schrijfgainy;
    int xdir, ydir, Adir, Bdir, ticka, tickb, refA,refB, errA, errB;
    int Aboven, Aonder, Bboven,Bonder, Astart, Bstart;
//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;
    kpc=1.0*0.1545;
    kdc=0.0*2.8*pow(10.0,-3.0);
    kic=0.1*1.0;
    kp=1*0.1545;
    kd=1.0*2.8*pow(10.0,-3.0);
    ki=0.05*1.0;
    rt=0.032805; //Straal tandwiel.
    gain=4.0; //Tegen filterverlies.
    emggrens=0.35;
    frictiona=0.65;
    frictionb=0.5;
    schrijfgainx=0.1;
    schrijfgainy=0.05;
    Astart=0;
    Bstart=1200; //Beginpositie pen in ticks.
    Ai1=0;
    Ad1=0;
    Bi1=0;
    Bd1=0;
    pc.baud(115200);
    Aboven=820;
    Aonder=165;
    Bboven=10900;
    Bonder=3400; //Grenzen in ticks. Komen overeen met hoeken van A4.

//Filtercoëfficienten.
    //High pass, 35Hz, 1e orde, 4 ms.
    numh1=0.680011076547878;
    numh2=-0.680011076547878;
    //denh1=1;
    denh2=-0.360022153095757;

    //Low pass, 5 Hz, 2e orde, 4 ms.
    numl1=0.003621681514929;
    numl2=0.007243363029857;
    numl3=0.003621681514929;
    //denl1=1;
    denl2=-1.822694925196308;
    denl3=0.837181651256023;

//Calibratie
    wait(4);
    while(calflag==true) {
        motor1.setPosition(Astart);
        motor2.setPosition(Bstart);
        while(looptimerflag != true);
        looptimerflag = false;
        refA=515;
        refB=3536; //515 - 3536 voor rechtsonder.
        while(calB==true) {
            tickb=motor2.getPosition();
            errB=refB-tickb;
            Bp=errB*kpc;
            Bd=(errB-Bd1)*kdc/ts;
            Bi=(Bi1+ts*errB)*kic;
            Bd1=Bd;
            Bi1=Bi;
            ctrlB=(Bi+Bp+Bd);
            for_B=(ctrlB)/1000.0;
            if(ctrlB<0.0) {
                Bdir=0;
            } else {
                Bdir=1;
            }
            keep_in_range(&for_B, -1.0,1.0);
            if (frictionflag==true) {
                for_B=abs(for_B)+frictionb;
                keep_in_range(&for_B, 0.0,0.1); 
                //Vreemd dat een bovengrens van 0.1 werkt ook al is friction groter dan 0.1. Bij hogere waarden gaat de motor te snel.
            }
            pwm_B.write(abs(for_B));
            motordirB.write(Bdir);
            if(errB<20) {
                calB=false;
                pwm_B.write(0.0);
            }
        }

        while(calA==true) {
            ticka=-1*motor1.getPosition(); //Omdat de motor op zijn kop staat.
            errA=refA-ticka;
            Ap=errA*kpc;
            Ad=(errA-Ad1)*kdc/ts;
            Ai=(Ai1+ts*errA)*kic;
            Ad1=Ad;
            Ai1=Ai;
            ctrlA=(Ai+Ap+Ad);
            for_A=(ctrlA)/1000.0;
            if(ctrlA<0) {
                Adir=1;
            } else {
                Adir=0;
            }
            keep_in_range(&for_A, -1,1);
            if (frictionflag==true) {
                for_A=abs(for_A)+frictiona;
                keep_in_range(&for_A, 0,0.1);
            }
            pwm_A.write(abs(for_A));
            motordirA.write(Adir);
            if(errA<20) {
                calA=false;
                pwm_A.write(0);
                calflag=false;
            }
        }
    }

//Meetloop.
    wait(1);
    //motor1.setPosition(515);
    //motor2.setPosition(3565);
    //Bovenstaande toegevoegd voor het geval de calibratie handmatig gedaan wordt.
    while(meetflag==true) {
        while(looptimerflag != true);
        looptimerflag = false;
//EMG lezen.
        //tr = triceps rechts.
        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;

//Gains om filter te compenseren.
        zx=(zbr*gain);
        zy=(zbl*gain);

//Grenzen en threshold voor emg.
        if (zx>1.0) {
            zx=0.99999;
        }
        if (zy>1.0) {
            zy=0.99999;
        }
        if (zx<emggrens) {
            zx=emggrens;
        }
        if (zy<emggrens) {
            zy=emggrens;
        }
        //Schaling om ook outputs lager dan de grenswaarde te krijgen.
        zx=zx-emggrens;
        zx=zx/(1.0-emggrens);
        zy=zy-emggrens;
        zy=zy/(1.0-emggrens);

//Richting omdraaien met triceps.
        if (ztr>0.15 && dirflagx == true) {
            dirflagx = false;
            xdir ^= 1;
            dirtimeout.attach(tricheck,1.0);
        }
        if (ztl>0.15 && dirflagy == true) {
            dirflagy = false;
            ydir ^= 1;
            dirtimeout.attach(tricheck,1.0);
        }

        //Bepalen juiste teken van verplaatsing.
        if (ydir==0) {
            zy=-1.0*zy;
            redled.write(1);
        }
        if (ydir==1) {
            redled.write(0);
        }
        if (xdir==0) {
            zx=-1.0*zx;
            greenled.write(1);
        }
        if (xdir==1) {
            greenled.write(0);
        }

        ticka=-1*motor1.getPosition(); //Zie calibratie.
        tickb=motor2.getPosition();

        //Omzetten naar schrijfsnelheid.
        vxuit=zx*schrijfgainx;
        vyuit=zy*schrijfgainy;
        xuit += ts*vxuit;
        yuit += ts*vyuit; //Integreren naar positie.

        //Begrenzing penpositie, getallen in meter.
        keep_in_range(&xuit,0.115,0.422);
        keep_in_range(&yuit,0.115,0.335);

        //Inversie kinematica en begrenzing motorhoeken.
        refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
        refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
        keep_in_rangeint(&refA,Aonder,Aboven);
        keep_in_rangeint(&refB,Bonder,Bboven);

        //Controllers
        errA=refA-ticka;
        errB=refB-tickb;
        Ap=errA*kp;
        Ad=(errA-Ad1)*kd/ts;
        Ai=(Ai1+ts*errA)*ki;
        Ad1=Ad;
        Ai1=Ai;
        ctrlA=(Ai+Ap+Ad);
        for_A=(ctrlA)/10.0;

        Bp=errB*kp;
        Bd=(errB-Bd1)*kd/ts;
        Bi=(Bi1+ts*errB)*ki;
        Bd1=Bd;
        Bi1=Bi;
        ctrlB=(Bi+Bp+Bd);
        for_B=(ctrlB)/1500.0;

        //Motorrichting bepalen.
        if(ctrlA<0) {
            Adir=1;
        } else {
            Adir=0;
        }
        if(ctrlB<0) {
            Bdir=0;
        } else {
            Bdir=1;
        }

        //PWM-begrenzing.
        keep_in_range(&for_A, -1.0,1.0);
        keep_in_range(&for_B, -1.0,1.0);

        //Wrijvingscompensatie.
        if (frictionflag==true) {
            for_A=abs(for_A)+frictiona;
            for_B=abs(for_B)+frictionb;
            keep_in_range(&for_A, frictiona,1);
            keep_in_range(&for_B, frictionb,1);
        }

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


            //Verscheidene prints die nuttig zijn gebeleken.
        if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
            //pc.printf(" %f %f \n\r",zx, zy);
            pc.printf(" %f %f %f %f\n\r",zbl,ztl,zbr,ztr);
            //pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB);
            //pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit*1000.0,yuit*1000.0);
            //pc.printf("A %f B %f\n\r",ctrlA,ctrlB);
            //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
            //pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B);
            //pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
            //pc.printf("A %i %i %i %i %f %f\n\r",ticka,refA,errA,ctrlA,for_A);
            //pc.printf("tr %f br %f tl %f bl %f\n\r",ztr,zbr,ztl,zbl);
            //pc.printf("%i\n",motor2.getPosition());
            //pc.printf("%f %f\n\r",for_A,for_B);
        }
    }
}

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