File content as of revision 7:5380d17310e8:

/* - - - - - - - - - - - - - - - - - - - - - - */
/*                                             */
/*  BRONCODE TEKENROBOT GROEP2: THE DRAWESOME  */
/*  -----------------------------------------  */
/*  Belinda Brandwacht   s1226290              */              
/*  Esther Keulers       s1255444              */  
/*  Maaike Schotman      s1274104              */  
/*  Gerjan Wolterink     s1197797              */  
/*  Roelof van Zwol      s1240811              */  
/*                                             */
/* - - - - - - - - - - - - - - - - - - - - - - */

//libraries aanroepen.
#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"

// Looptimerflag instellen

volatile bool looptimerflag;   

void setlooptimerflag(void)
{
    looptimerflag=true;
}


int main()
{
    //Communicatie met de pc
    MODSERIAL pc(USBTX,USBRX,64,1024);
    pc.baud(115200); //zet de baud in realterm ook op dit nummer. 

    //Benoemen inputs van de drie rode knoppen op de robot
    DigitalIn knop1(PTC2);
    DigitalIn knop2(PTB3);
    DigitalIn knop3(PTB2);
    
    //Input mode van de knoppen instellen. 
    knop1.mode(PullUp);
    knop2.mode(PullUp);
    knop3.mode(PullUp);
    
    AnalogIn    emg1(PTB0); //Analog input emg1
    AnalogIn    emg2(PTB1); //Analog input emg2

    //Benoemen pinnen waarop de encoder van de motoren is aangeloten.
    Encoder motor1(PTD0,PTC9); 
    Encoder motor2(PTD2,PTC8);

    /* PWM control to motor */
    PwmOut pwm_motor1(PTA12);
    PwmOut pwm_motor2(PTA5);
   
    /* Direction pin */
    DigitalOut motor1dir(PTD3);
    DigitalOut motor2dir(PTD1);
    

    //Variabelen voor menu
    int state = 1;

    // Met bool wordt gezegd dat variablen 'true' of 'false' kunnen zijn. 
    bool calibratie_rust = false;
    bool calibratie_max  = false;
    bool calibratie_motor = false;
    bool meting = false;

    //Variabelen EMG/motoren
    double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
    double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;

    // Variabelen benoemen voor regelaar motor.
    double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm,pi;
    double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, dri, dri_1, utot_r, inputsinus;
    double motor1_maxu, motor2_maxu;

    
    pi=3.14159265359;

    e1= 0.855930601814815;
    e2=  -1.711861203629630;
    e3= 0.855930601814815;
    f1=-1.690996376887443;
    f2= 0.732726030371816;

    g1= 0.098259168204717*pow(10.0,-4.0);
    g2= 0.196518336409435*pow(10.0,-4.0);
    g3=0.098259168204717*pow(10.0,-4.0);
    h1= -1.991114292201654;
    h2=0.991153595868935;

    be1=0.914969144113083;
    be2= -1.829938288226165;
    be3= 0.914969144113083;
    bf1=-1.822694925196308;
    bf2=0.837181651256022;

    bg1= 0.246193004641015*pow(10.0,-5.0);
    bg2= 0.492386009282031 *pow(10.0,-5.0);
    bg3= 0.246193004641015*pow(10.0,-5.0);
    bh1= -1.995557124345789;
    bh2= 0.995566972065975;


    y1 = 0; /*Initialize y(n-1) = 0*/
    y2 = 0; /*Initialize y(n-2) = 0*/
    z1 = 0; /*Initialize z(n-1) = 0*/
    z2 = 0; /*Initialize z(n-2) = 0*/
    zabs1 = 0; /*Initialize zabs(n-1) = 0*/
    zabs2 = 0; /*Initialize zabs(n-2) = 0*/
    w1 = 0; /*Initialize w(n-1) = 0*/
    w2 = 0; /*Initialize w(n-2) = 0*/

    by1 = 0; /*Initialize by(n-1) = 0*/
    by2 = 0; /*Initialize by(n-2) = 0*/
    bz1 = 0; /*Initialize bz(n-1) = 0*/
    bz2 = 0; /*Initialize bz(n-2) = 0*/
    bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
    bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
    bw1 = 0; /*Initialize bw(n-1) = 0*/
    bw2 = 0; /*Initialize bw(n-2) = 0*/
    const float Ts = 0.001; /*Sample Time, const(ant)*/

//Constanten voor tekening: EMGdeel
    xbegin= 0; //beginpositie
    ybegin=0; //beginpositie
    qbegin=5.50;  //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
    MaxsnelheidV=150.0;     //in mm/s
    MaxsnelheidD=0.5*pi;    //in rad/s eerst 0.26 rad/s (15 deg/sec)
    
//constanten regelaar
    kp_r = 0.006;
    ki_r = 0.005;

    Ticker looptimer;
    looptimer.attach(setlooptimerflag,Ts);

    while (true) {              //oneindige while loop

        //Loop die allen gestart wordt als de looptimer is verlopen. 
        while(looptimerflag != true);
        looptimerflag = false;


// state 1 RUST
        if (state == 1) {

            pc.printf("state 1 RUST  |     knop1 = cal_motor     knop2 = MENU EMG     knop3 = Tekenen \n\r");


            if (knop1 == false ) {  //als knop 1 is ingedrukt
                state=2;
                wait(0.05);        //wacht goed contact knopje

                while(knop1 == false) {}    //wacht tot knop is losgelaten
                wait(0.05);                 // wacht goed contact knopje

            }

            if (knop2 == false ) {  //als knop 2 wordt ingedrukt: state is 3
                state=3;
                wait(0.05);

                while(knop2 == false) {}
                wait(0.05);
            }
            if (knop3 == false && calibratie_rust == true && calibratie_max  == true && calibratie_motor == true) {
                state=6;    //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn.
                wait(0.05);

                while(knop3 == false) {}
                wait(0.05);
                meting=true;

            }
            if (knop3 == false && state !=6) {
                wait(0.05);
                pc.printf("Niet alle calibraties uitgevoerd! \n");
                wait(1);
            }


        }


//state2 CAL MOTOR
        if (state == 2) {
            pc.printf("state 2 cal_motor    |    knop1 = terug naar rust \n\r");

            //Als de robot arm met pen met de hand op de begin posisie gezet is moet knop 1 weer worden ingedrukt.
            if (knop1 == false ) {
                state=1;
                wait(0.05);

                while(knop1 == false) {}
                wait(0.05);

                motor1.setPosition(200.0);   //Zeg dat de motor die de arm verplaatst 200 counts heeft, dit komt overeen met (200/CPR)/gearatio = (200/CPR)/50=0.125 dat komt overeen met een hoek van pi/4
                motor2.setPosition(597.15);  //Zeg dat de motor die de loper verplaatst 597.15 counts heeft, dit komt overeen met r=98,71 mm

                x=0; //Zeg dat de pen op x=0 staat
                y=0; //Zeg dat de pen op y=0 staat

                dri_1=0; //vorige integraal van de fout, 0 voor de eerste keer. 

                calibratie_motor = true;  //zeg dat de calibratie van de motor is uitgevoerd. 


            }
        }
// state 3 MENU EMG
        if (state == 3) {
            pc.printf("state 3 MENU EMG | knop1= cal EMG rust  knop2= rust   knop3= cal EMG max  \n\r");


            if (knop2 == false ) {  // terug naar state 1
                state=1;
                wait(0.05);

                while(knop2 == false) {}
                wait(0.05);
            }

            if (knop1 == false ) {  // naar state 4
                state=4;
                wait(0.05);

                while(knop1 == false) {}
                wait(0.05);

                // begingrenswaarden stellen:
                GrenswaardeV=0; 
                GrenswaardeD=0;
                meting=true; //Zet de meting 'aan'
            }

            if (knop3 == false ) {  // naar state 5
                state=5;
                wait(0.05);

                while(knop3 == false) {}
                wait(0.05);

                // begingrenswaarden stellen:
                MaxwaardeV=0;
                MaxwaardeD=0;
                meting=true;

            }


        }


        //METEN VAN EMG
        if (meting == true) {
            //  pc.printf("TEST CODE: Meting in gang\n");
            yy = emg1.read(); //<measurement function, read from pin>
            byy= emg2.read();

            z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
            bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/

            y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
            y1= yy; /*y(n) will be y(n-1) on next loop*/
            z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
            z1 = z; /*z(n) will be z(n-1) on next loop*/

            by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
            by1= byy; /*y(n) will be y(n-1) on next loop*/
            bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
            bz1 = bz; /*z(n) will be z(n-1) on next loop*/

            zabs=abs(z);/*rectify*/
            bzabs=abs(bz);/*rectify*/

            w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
            bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/

            zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
            zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
            w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
            w1 = w; /*w(n) will be w(n-1) on next loop*/

            bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
            bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
            bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
            bw1 = bw; /*w(n) will be w(n-1) on next loop*/

            //pc.printf("%f,%f\n",w,bw);


            if (w>1.0) {
                w=1.0;
            }
            if (bw>1.0) {
                bw=1.0;
            }

        }



// state 4 EMG CAL IN RUST
        if (state == 4) {
            pc.printf("state 4 cal EMG RUST    |    knop1= MEMNU EMG \n\r");
            pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
            
            // Nieuwe grenswaarden bepalen
            if (w > GrenswaardeV) {
                GrenswaardeV = w;
            }
            if (bw > GrenswaardeD) {
                GrenswaardeD = bw;
            }
            if (knop1 == false ) {  // terug naar state 1
                state=3;
                wait(0.05);

                while(knop1 == false) {}
                wait(0.05);
                calibratie_rust = true;  //Zeg dat de calibratie van de emg in rust in uitgevoerd. 
                meting=false;            //Zet de meting weer 'uit'

            }
        }
// state 5 EMG CAL MAX
        if (state == 5) {
            pc.printf("state 5 cal EMG MAX |    knop3= EMG MENU \n\r");
            pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
            // Nieuwe maxwaarden bepalen
            if (w > MaxwaardeV) {
                MaxwaardeV = w;
            }
            if (bw > MaxwaardeD) {
                MaxwaardeD = bw;
            }

            // einde maxwaarde bepalen:
            if (knop3 == false ) {  // terug naar state 3
                state=3;
                wait(0.05);

                while(knop3 == false) {}
                wait(0.05);
                calibratie_max  = true;
                meting=false;

            }

        }

// state 6  TEKENEN

        if (state == 6) {
            // pc.printf("state 6 tekenen  |    knop3= rust \n\r");

            // EMG
            BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
            BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)

            SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
            SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik

            if (SnelheidV<0) {
                SnelheidV=0;
            }
            if (SnelheidD<0) {
                SnelheidD=0;
            }
            if (SnelheidV>MaxsnelheidV) {
                SnelheidV=MaxsnelheidV;
            }
            if (SnelheidD>MaxsnelheidV) {
                SnelheidD=MaxsnelheidV;
            }

            deltas = SnelheidV*Ts;  //afstand tussen twee meetpunten
            deltaq = SnelheidD*Ts;  //verandering hoek tov. de rechtdoor
            q = deltaq+qbegin;           // hoekverandering tov. het assenstelsel

            deltay = deltas*cos(q);  //veranding van pen in de y-richting
            deltax = deltas*sin(q);  // verandering van pen in de x-richting

            x = xbegin+deltax;       // nieuwe postie pen, x-coördinaat
            y = ybegin+deltay;       // nieuwe positie pen, y-coöridnaat

            if (x>297) {       //grenzen A4
                x=297;
            }
            if (y>210) {
                y=210;
            }

            if (x<0) {       //grenzen A4
                x=0;
            }
            if (y<0) {
                y=0;
            }

            xbegin=x;    // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
            ybegin=y;    // y zal ybgin worden in de volgende loop  oftewel:(y(n) will be y(n-1) on next loop)
            qbegin=q;   // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)

            pc.printf("bV=%f, bD=%f, sV=%f, sD=%f, x=%f, y=%f, hoek=%f\n",BereikV,BereikD,SnelheidV,SnelheidD,x,y,(q*57.3));


// motoren

            if (y>210.0) {
                y=210.0;
            }
            if(y<=0.0) {
                y=0.0;
            }
            if(x>297.0) {
                x=297.0;
            }
            if(x<=0.0) {
                x=0.0;
            }

            
            //x en y omzetten in hoek en straal. 
            theta = atan((x+69.80)/(y+69.80));
            r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );

            //lees de hoek en de straal af uit de motoren. 
            theta_pen   = motor1.getPosition()  *   ((.5*pi)/400.0);  //Zet de CPR om in een hoek.
            r_pen       = motor2.getPosition()  *   (461.34/2790.13); //zet de CPRM om in een straaal van de pen. 

            // Berekende het verschil in gewensete en werkelijk hoek en straal. 
            dtheta  = (theta - theta_pen);
            dr      = (r - r_pen);

            //REGELAAR
            up_r = kp_r * dr;                   //P-actie
            dri  = dri_1 + dr*Ts;
            ui_r = dri_1 * ki_r;                //I-actie
            utot_r = up_r + ui_r;               //

            dri_1 =  dri;                       //nieuwe waardes oud maken.

            theta_pwm   = (dtheta)*3.0;
            r_pwm       = (utot_r/1.0);

            //NAAR MOTOR

            //Zorgen dat pwm tussen -1 en 1 blijft.
            if(theta_pwm > 1) {
                theta_pwm=1;
            }
            if(theta_pwm < -1) {
                theta_pwm=-1;
            }
            if(r_pwm > 1) {
                r_pwm=1;
            }
            if(r_pwm < -1) {
                r_pwm=-1;
            }

            // Bepaal richting waarin motoren moeten draaien
            if(theta_pwm > 0)
                motor1dir.write(1);
            else
                motor1dir.write(0);
            if(r_pwm > 0)
                motor2dir.write(1);
            else
                motor2dir.write(0);

            // Testcode - print naar pc
            //pc.printf("t=%.3f   dt=%.3f  tpwm=%.3f  |  r=%.3f   dr=%.3f  rpwm=%.3f  inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus);

            //schrijf PWM naar motor
            pwm_motor1.write(abs(theta_pwm));
            pwm_motor2.write(abs(r_pwm));

            // terug naar state 1 knop
            if (knop3 == false ) {  // terug naar state 1
                state=1;
                meting=false;
                wait(0.05);

                while(knop3 == false) {}
                wait(0.05);
            }

        }

    } //sluiten oneindige while loop

}   //afsluiten main