Broncode projectgroep 2 Belinda Brandwacht s1226290 Esther Keulers s1255444 Maaike Schotman s1274104 Gerjan Wolterink s1197797 Roelof van Zwol s1240811

Dependencies:   Encoder MODSERIAL mbed

Fork of Begintotaalscript by Esther Keulers

Files at this revision

API Documentation at this revision

Comitter:
gerjan
Date:
Wed Nov 06 17:02:58 2013 +0000
Parent:
6:d4f355d72f66
Commit message:
Broncode projectgroep 2; ; Belinda Brandwacht s1226290; Esther Keulers s1255444; Maaike Schotman s1274104; Gerjan Wolterink s1197797; Roelof van Zwol s1240811

Changed in this revision

begintotaalscript.cpp Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r d4f355d72f66 -r 5380d17310e8 begintotaalscript.cpp
--- a/begintotaalscript.cpp	Wed Nov 06 16:37:36 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,500 +0,0 @@
-/* - - - - - - - - - - - - - - - - - - - - - - */
-/*                                             */
-/*  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 .25 pi
-                motor2.setPosition(597.15);  //
-
-                x=0;
-                y=0;
-
-                dri_1=0;
-
-                calibratie_motor = true;
-
-
-
-
-            }
-        }
-// 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;
-            }
-
-            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);
-
-            wait(Ts); /*When finished wait (maybe a bit shorter?)*/
-
-            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;
-                meting=false;
-
-            }
-        }
-// 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;
-            }
-
-            // Omzetten hoek en straal
-            //theta   = atan(y/x)+0.25*pi;
-            //r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
-
-            theta = atan((x+69.80)/(y+69.80));
-            r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );
-
-
-            theta_pen   = motor1.getPosition()  *   ((.5*pi)/400.0);
-            r_pen       = motor2.getPosition()  *   (461.34/2790.13);
-
-
-            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);
-
-            // 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
\ No newline at end of file
diff -r d4f355d72f66 -r 5380d17310e8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 06 17:02:58 2013 +0000
@@ -0,0 +1,496 @@
+/* - - - - - - - - - - - - - - - - - - - - - - */
+/*                                             */
+/*  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
\ No newline at end of file