2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range

Dependencies:   Encoder MODSERIAL TextLCD mbed mbed-dsp

Revision:
25:71e52c56be13
Parent:
24:a1fdc830f96c
Child:
26:438a498e5526
--- a/main.cpp	Sun Nov 02 19:42:31 2014 +0000
+++ b/main.cpp	Mon Nov 03 11:13:33 2014 +0000
@@ -6,13 +6,13 @@
 
 //Constanten definiëren en waarde geven
 #define SAMPLETIME_REGELAAR         0.005       //Sampletijd ticker regelaar motor
+#define SAMPLETIME_EMG              0.005       //Sampletijd ticker EMG meten
 #define KP_arm1                     0.02        //Factor proprotionele regelaar arm 1
-#define KI_arm1                     0         //Factor integraal regelaar arm 1
-#define KD_arm1                     0          //Factor afgeleide regelaar arm 1
+#define KI_arm1                     0           //Factor integraal regelaar arm 1
+#define KD_arm1                     0           //Factor afgeleide regelaar arm 1
 #define KP_arm2                     0.01        //Factor proprotionele regelaar arm 2
 #define KI_arm2                     0           //Factor integraal regelaar arm 2
 #define KD_arm2                     0           //Factor afgeleide regelaar arm 2
-#define SAMPLETIME_EMG              0.005       //Sampletijd ticker EMG meten
 
 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
 //High Pass filter
@@ -54,16 +54,17 @@
 AnalogIn        EMG_tri(PTB0);                  //Uitlezen EMG signaal triceps
 //Blauw op 3,3 V en groen op GND, voeding beide encoders
 
-Ticker ticker_regelaar;                 //Ticker voor regelaar motor
-Ticker ticker_EMG;                      //Ticker voor EMG meten
+Ticker ticker_regelaar;                 //Ticker voor flag veranderen referentie
+Ticker ticker_EMG;                      //Ticker voor flag EMG meten
+Ticker ticker_motor_arm1_pid;           //Ticker voor PID regelaar motor arm 1
+Ticker ticker_motor_arm2_pid;           //Ticker voor PID regelaar motor arm 2
+
 Timer biceps_kalibratie;                //Timer voor kalibratiemeting EMG biceps
 Timer triceps_kalibratie;               //Timer voor kalibratiemeting EMG triceps
-Ticker ticker_motor_arm1_pid;
-Ticker ticker_motor_arm2_pid;
-Timer EMG;
+Timer EMG;                              //Timer voor aantal seconden EMG meten
 
 //States definiëren
-enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT, WACHT, THUISPOSITIE_MIDDEN};     //Alle states benoemen, ieder krijgt een getal beginnend met 0
+enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT, WACHT, THUISPOSITIE_MIDDEN, THUISPOSITIE_RECHTS};     //Alle states benoemen, ieder krijgt een getal beginnend met 0
 uint8_t state = RUST;                   //State is rust aan het begin
 
 //Gedefinieerde datatypen en naamgeving en beginwaarden
@@ -79,12 +80,11 @@
 float afgeleide_arm2;                   //Afgeleide actie van regelaar arm 2
 float xb;                               //Gemeten EMG waarde biceps
 float xt;                               //Gemeten EMG waarde triceps
-
 float referentie_arm1 = 0;
 float referentie_arm2 = 0;
-
 float t = 0;
 
+//Gedefinieerde filters met constanten en gefilterde waarden
 arm_biquad_casd_df1_inst_f32 highpass_biceps;               //Highpass_biceps IIR filter in direct form 1
 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3};     //Filtercoëfficiënten van het filter
 float highpass_biceps_states[8];                            //Aantal states van het filter, het aantal y(n-x) en x(n-x)
@@ -142,14 +142,14 @@
     }
 }
 
-void motor_arm1_pid()
+void motor_arm1_pid()           //PID regelaar van motor arm 1
 {
     error_arm1_kalibratie = (referentie_arm1 - puls_motor_arm1.getPosition());                //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
     integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR;                        //Integraal deel van regelaar
     afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR;                   //Afgeleide deel van regelaar
     pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
-    vorige_error_arm1 = error_arm1_kalibratie;
-    keep_in_range(&pwm_to_motor_arm1, -1, 1);
+    vorige_error_arm1 = error_arm1_kalibratie;                      //Vorige error is de volgende keer de huidige error van deze keer
+    keep_in_range(&pwm_to_motor_arm1, -1, 1);                       //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben
 
     if (pwm_to_motor_arm1 > 0) {                //Als PWM is positief, dan richting 1
         dir_motor_arm1.write(0);
@@ -160,14 +160,14 @@
     pwm_motor_arm1.write(fabs(pwm_to_motor_arm1));      //Output PWM naar motor is de absolute waarde van de berekende PWM
 }
 
-void motor_arm2_pid()
+void motor_arm2_pid()           //PID regelaar van motor arm 2
 {
     error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition());                //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
     integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR;                        //Integraal deel van regelaar
     afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR;                   //Afgeleide deel van regelaar
     pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
-    vorige_error_arm2 = error_arm2_kalibratie;
-    keep_in_range(&pwm_to_motor_arm2, -1, 1);                                                           //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+    vorige_error_arm2 = error_arm2_kalibratie;                      //Vorige error is de volgende keer de huidige error van deze keer
+    keep_in_range(&pwm_to_motor_arm2, -1, 1);                       //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben
 
     if (pwm_to_motor_arm2 > 0) {                        //Als PWM is positief, dan richting 1
         dir_motor_arm2.write(1);
@@ -204,7 +204,7 @@
 
 }
 
-void EMG_meten()
+void EMG_meten()                //Meet de EMG van de biceps en de triceps
 {
     xb = EMG_bi.read();         //EMG signaal biceps uitlezen
     filter_biceps();            //EMG signaal biceps filteren
@@ -215,8 +215,8 @@
 
 int main()                          //Main script
 {
-    ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR);
-    ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+    ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR);           //Iedere SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor
+    ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);           //Iederen SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor
 
     while(1) {                      //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
         pc.baud(38400);             //PC baud rate is 38400 bits/seconde
@@ -232,7 +232,7 @@
                 wait(2);                                //Twee seconden wachten
                 lcd.cls();                              //Maak LCD scherm leeg
                 pc.printf("RUST afgerond\n\r");         //Tekst voor controle Aanzetten
-                state = KALIBRATIE_ARM1;                //State wordt kalibratie arm 1, zo door naar volgende onderdeel
+                state = KALIBRATIE_ARM1;                //Door naar de volgende state
                 break;                                  //Stopt acties in deze case
             }
 
@@ -246,17 +246,16 @@
                     while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    referentie_arm1 = referentie_arm1 + 180.0/200.0;
-
-                    pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition());       //Huidig aantal getelde pulsen van de encoder naar pc sturen
-                    pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1);                            //Huidige PWM waarde naar motor naar pc sturen
-                    pc.printf("referentie1 %f\n\r", referentie_arm1);
+                    referentie_arm1 = referentie_arm1 + 180.0/200.0;            //Referentie loopt in één seconde op tot het gewenste aantal pulsen
 
-                    if (referentie_arm1 >= 180) {
-                        referentie_arm1 = 180;
-                        state = KALIBRATIE_ARM2;
+                    pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition());          //Huidig aantal getelde pulsen van de encoder naar pc sturen
+                    pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1);                                  //Huidige PWM waarde naar motor naar pc sturen
+                    pc.printf("referentie1 %f\n\r", referentie_arm1);                               //Huidige referentie naar pc sturen
+
+                    if (puls_motor_arm1.getPosition() >= 180) {     //Als het gewenste aantal pulsen behaald is
+                        referentie_arm1 = 180;                      //Blijft de referentie op dat aantal pulsen staan
+                        state = KALIBRATIE_ARM2;                    //Door naar de volgende state
                     }
-
                 }
                 wait(1);                                            //Een seconde wachten
                 break;                                              //Stopt acties in deze case
@@ -272,20 +271,18 @@
                     while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    referentie_arm2 = referentie_arm2 + 123.0/200.0;
-
-                    pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition());       //Huidig aantal getelde pulsen van de encoder naar pc sturen
-                    pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2);                            //Huidige PWM waarde naar motor naar pc sturen
-                    pc.printf("referentie2 %f\n\r", referentie_arm2);
+                    referentie_arm2 = referentie_arm2 + 123.0/200.0;            //Referentie loopt in één seconde op tot het gewenste aantal pulsen
 
-                    if(referentie_arm2 >= 123) {
-                        referentie_arm2 = 123;
-                        state = EMG_KALIBRATIE_BICEPS;
+                    pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition());          //Huidig aantal getelde pulsen van de encoder naar pc sturen
+                    pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2);                                  //Huidige PWM waarde naar motor naar pc sturen
+                    pc.printf("referentie2 %f\n\r", referentie_arm2);                               //Huidige referentie naar pc sturen
+
+                    if(puls_motor_arm2.getPosition() >= 123) {        //Als het gewenste aantal pulsen behaald is
+                        referentie_arm2 = 123;                      //Blijft de referentie op dat aantal pulsen staan
+                        state = EMG_KALIBRATIE_BICEPS;              //Door naar de volgende state
                     }
-
                 }
                 wait(1);                                            //Een seconde wachten
-                //ticker_regelaar.detach();                           //Ticker detachten, ticker doet nu niks meer
                 break;                                              //Stopt acties in deze case
             }
 
@@ -296,8 +293,8 @@
                 lcd.printf(" SPAN IN 5 SEC. ");         //Tekst op LCD scherm
                 lcd.locate(0,1);                        //Zet tekst op tweede regel
                 lcd.printf(" 2 X BICEPS AAN ");         //Tekst op LCD scherm
-                wait(1);
-                lcd.cls();
+                wait(1);                                //Een seconden wachten
+                lcd.cls();                              //LCD scherm leegmaken
 
                 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 2, highpass_biceps_const, highpass_biceps_states);        //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
                 arm_biquad_cascade_df1_init_f32(&notch_biceps, 2, notch_biceps_const, notch_biceps_states);                 //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
@@ -346,28 +343,27 @@
                     }
                 }
 
-                xbt = xbfmax * 0.95;             //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
-                pc.printf("maximale biceps %f", xbfmax);
+                xbt = xbfmax * 0.90;             //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
+                pc.printf("maximale biceps %f", xbfmax);    //Maximale gefilterde EMG waarde naar pc sturen
                 pc.printf("threshold is %f\n\r", xbt);      //Thresholdwaarde naar pc sturen
                 state = EMG_KALIBRATIE_TRICEPS;             //Gaat door naar kalibratie van de EMG van de triceps
                 break;                                      //Stopt alle acties in deze case
             }
 
-            case EMG_KALIBRATIE_TRICEPS: {
+            case EMG_KALIBRATIE_TRICEPS: {              //Kalibratie triceps voor bepalen threshold
                 pc.printf("EMG__KALIBRATIE_TRICEPS\n\r");           //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen
 
                 lcd.locate(0,0);                        //Zet de tekst op de eerste regel
                 lcd.printf(" SPAN IN 5 SEC. ");         //Tekst op LCD scherm
                 lcd.locate(0,1);                        //Zet tekst op eerste regel
                 lcd.printf(" 2 X TRICEPS AAN");         //Tekst op LCD scherm
-                wait(1);
-                lcd.cls();
+                wait(1);                                //Een seconde wachten
+                lcd.cls();                              //LCD scherm leegmaken
 
                 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 2, highpass_triceps_const, highpass_triceps_states);     //Highpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
                 arm_biquad_cascade_df1_init_f32(&notch_triceps, 2, notch_triceps_const, notch_triceps_states);              //Notchfilter triceps met bijbehorende filtercoëfficiënten en states definiëren
                 arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 2, lowpass_triceps_const, lowpass_triceps_states);        //Lowpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
 
-                //ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);           //Deze ticker loopt nog
                 triceps_kalibratie.start();                                         //Timer aanzetten die de tijd meet vanaf dit punt
 
                 while(triceps_kalibratie.read() <= 5) {             //Zolang de timer nog geen 5 seconden heeft gemeten
@@ -411,13 +407,13 @@
                 }
 
                 xtt = xtfmax * 0.90;             //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
-                pc.printf("maximale triceps %f", xtfmax);
+                pc.printf("maximale triceps %f", xtfmax);   //Maximale gefilterde EMG waarde naar pc sturen
                 pc.printf("threshold is %f\n\r", xtt);      //Thresholdwaarde naar pc sturen
                 state = START;                              //Gaat door naar het slaan, kalibratie nu afgerond
                 break;                                      //Stopt alle acties in deze case
             }
 
-            case START: {
+            case START: {               //Eerste keuze maken voor doel
                 lcd.locate(0,0);                        //Zet tekst op eerste regel
                 lcd.printf("      START     ");         //Tekst op LCD scherm
 
@@ -432,37 +428,37 @@
                     xt = EMG_tri.read();        //EMG signaal triceps uitlezen
                     filter_triceps();           //EMG signaal triceps filteren
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = B;              //Ga door naar state B
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = T;              //Ga door naar state T
                     }
                 }
                 break;                          //Stopt met de acties in deze case
             }
 
-            case B: {
+            case B: {                   //Tweede keuze maken voor doel
                 lcd.locate(0,0);                        //Zet tekst op eerste regel
                 lcd.printf("        B       ");         //Tekst op LCD scherm
                 pc.printf("B\n\r");                     //Controle naar pc sturen
 
-                EMG.start();
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == B) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BB;             //Ga door naar state BB
                     }
                     if(xtf>= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
@@ -472,80 +468,80 @@
                 break;                          //Stop met alle acties in deze case
             }
 
-            case T: {
+            case T: {                   //Tweede keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("        T       ");     //Tekst op LCD scherm
                 pc.printf("T\n\r");                 //Controle naar pc sturen
 
-                EMG.start();
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == T) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TB;             //Ga door naar state TB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = TT;             //Ga door naar state TT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BB: {
+            case BB: {                  //Derde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       BB       ");     //Tekst op LCD scherm
                 pc.printf("BB\n\r");                //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == BB) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BBB;            //Ga door naar state BBB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = BBT;            //Ga door naar state BBT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BT: {
+            case BT: {                  //Derde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       BT       ");     //Tekst op LCD scherm
                 pc.printf("BT\n\r");                //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == BT) {
@@ -554,103 +550,103 @@
 
                     EMG_meten();
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BTB;            //Ga door naar state BTB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = BTT;            //Ga door naar state BTT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TB: {
+            case TB: {                  //Derde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       TB       ");     //Tekst op LCD scherm
                 pc.printf("TB\n\r");                //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == TB) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TBB;            //Ga door naar state TBB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = TBT;            //Ga door naar state TBT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TT: {
+            case TT: {                  //Derde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       TT       ");     //Tekst op LCD scherm
                 pc.printf("TT\n\r");                //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == TT) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TTB;            //Ga door naar state TTB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = TTT;            //Ga door naar state TTT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BBB: {
+            case BBB: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       BBB      ");     //Tekst op LCD scherm
                 pc.printf("BBB\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == BBB) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BBBB;           //Ga door naar state BBBB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = BBBT;           //Ga door naar state BBBT
                     }
                 }
@@ -658,150 +654,150 @@
             }
 
 
-            case BBT: {
+            case BBT: {                 //Vierde keuze maken in doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       BBT      ");     //Tekst op LCD scherm
                 pc.printf("BBT\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == BBT) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BBTB;           //Ga door naar state BBTB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = BBTT;           //Ga door naar state BBTT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BTB: {
+            case BTB: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       BTB      ");     //Tekst op LCD scherm
                 pc.printf("BTB\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == BTB) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BTBB;           //Ga door naar state BTBB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = BTBT;           //Ga door naar state BTBT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BTT: {
+            case BTT: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       BTT      ");     //Tekst op LCD scherm
                 pc.printf("BTT\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == BTT) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = BTTB;           //Ga door naar state BTTB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = BTTT;           //Ga door naar state BTTT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TBB: {
+            case TBB: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       TBB      ");     //Tekst op LCD scherm
                 pc.printf("TBB\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == TBB) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
-                    if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+                    if(xbf >= xbt) {            //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TBBB;           //Ga door naar state TBBB
                     }
-                    if(xtf >= xtt) {             //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+                    if(xtf >= xtt) {            //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
                         state = TBBT;           //Ga door naar state TBBT
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TBT: {
+            case TBT: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       TBT      ");     //Tekst op LCD scherm
                 pc.printf("TBT\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == TBT) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
                     if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TBTB;           //Ga door naar state TBTB
@@ -813,26 +809,26 @@
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TTB: {
+            case TTB: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       TTB      ");     //Tekst op LCD scherm
                 pc.printf("TTB\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == TTB) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
                     if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TTBB;           //Ga door naar state TTBB
@@ -844,26 +840,26 @@
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TTT: {
+            case TTT: {                 //Vierde keuze maken voor doel
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("       TTT      ");     //Tekst op LCD scherm
                 pc.printf("TTT\n\r");               //Controle naar pc sturen
 
-                EMG.reset();
-                EMG.start();
+                EMG.reset();            //Timer resetten
+                EMG.start();            //Timer aanzetten voor EMG meten
 
-                while(EMG.read() <= 3) {
+                while(EMG.read() <= 2) {                            //Timer nog geen drie seconden gemeten
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
                 }
 
                 while(state == TTT) {
                     while(regelaar_EMG_flag != true);               //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_EMG_flag = false;                      //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    EMG_meten();
+                    EMG_meten();        //EMG meten van biceps en triceps
 
                     if(xbf >= xbt) {             //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
                         state = TTTB;           //Ga door naar state TTTB
@@ -875,140 +871,247 @@
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BBBB: {
+            case BBBB: {                //Motoraansturing voor doel rechtsonder
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BBBB      ");     //Tekst op LCD scherm
                 pc.printf("BBBB\n\r");              //Controle naar pc sturen
 
                 while(state == BBBB) {
-                    //Motoractie
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    while(puls_motor_arm1.getPosition() > -84) {
+                        referentie_arm1 = referentie_arm1 - 264.0/200.0;
+                        pc.printf("referentie arm 1 %f ", referentie_arm1);
+                        pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                        pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
+                    }
+                    pc.printf("arm 1 op positie");
+
+                    while(puls_motor_arm2.getPosition() < 211) {
+                        referentie_arm2 = referentie_arm2 + 88.0/200.0;
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
+                    }
+                    pc.printf("arm2 op positie");
+
+                    while(puls_motor_arm2.getPosition() > 36) {
+
+                        referentie_arm2 = -0.5 * 100000 * t * t;
+                        t = t + 0.005;
+
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+                        pc.printf("t is %f\n\r", t);
+                    }
+
+                    if(puls_motor_arm2.getPosition() <= 36) {
+                        wait(1);
+                        while(puls_motor_arm2.getPosition() < 211) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
+
+                            pc.printf("referentie arm 2 %f ", referentie_arm2);
+                            pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                            pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+                        }
+
+                        if(puls_motor_arm2.getPosition() >= 211) {
+                            pc.printf("staat stil\n\r");
+                            state = THUISPOSITIE_RECHTS;
+                        }
+                    }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BBBT: {
+            case BBBT: {                //Geen motoraansturing
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BBBT      ");     //Tekst op LCD scherm
                 pc.printf("BBBT\n\r");              //Controle naar pc sturen
 
                 while(state == BBBT) {
-                    //Motoractie
+                    lcd.printf("   GEEN DOEL    ");
+                    wait(1);
+                    state = WACHT;
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BBTB: {
+            case BBTB: {                //Motoraansturing voor doel rechtsmidden
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BBTB      ");     //Tekst op LCD scherm
                 pc.printf("BBTB\n\r");              //Controle naar pc sturen
 
                 while(state == BBTB) {
-                    //Motoractie
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    while(puls_motor_arm1.getPosition() > -84) {
+                        referentie_arm1 = referentie_arm1 - 264.0/200.0;
+                        pc.printf("referentie arm 1 %f ", referentie_arm1);
+                        pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                        pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
+                    }
+                    pc.printf("arm 1 op positie");
+
+                    while(puls_motor_arm2.getPosition() < 211) {
+                        referentie_arm2 = referentie_arm2 + 88.0/200.0;
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
+                    }
+                    pc.printf("arm2 op positie");
+
+
+
+                    while(puls_motor_arm2.getPosition() > 36) {
+
+                        referentie_arm2 = -0.5 * 150000 * t * t;
+                        t = t + 0.005;
+
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+                        pc.printf("t is %f\n\r", t);
+                    }
+
+                    if(puls_motor_arm2.getPosition() <= 36) {
+                        wait(1);
+                        while(puls_motor_arm2.getPosition() < 211) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
+
+                            pc.printf("referentie arm 2 %f ", referentie_arm2);
+                            pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                            pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+                        }
+
+                        if(puls_motor_arm2.getPosition() >= 211) {
+                            pc.printf("staat stil\n\r");
+                            state = THUISPOSITIE_RECHTS;
+                        }
+                    }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BBTT: {
+            case BBTT: {                //Motoraansturing voor doel rechtsboven
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BBTT      ");     //Tekst op LCD scherm
                 pc.printf("BBTT\n\r");              //Controle naar pc sturen
 
                 while(state == BBTT) {
-                    //Motoractie
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    while(puls_motor_arm1.getPosition() > -84) {
+                        referentie_arm1 = referentie_arm1 - 264.0/200.0;
+                        pc.printf("referentie arm 1 %f ", referentie_arm1);
+                        pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                        pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
+                    }
+                    pc.printf("arm 1 op positie");
+
+                    while(puls_motor_arm2.getPosition() < 211) {
+                        referentie_arm2 = referentie_arm2 + 88.0/200.0;
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
+                    }
+                    pc.printf("arm2 op positie");
+
+                    while(puls_motor_arm2.getPosition() > 36) {
+
+                        referentie_arm2 = -0.5 * 200000 * t * t;
+                        t = t + 0.005;
+
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+                        pc.printf("t is %f\n\r", t);
+                    }
+
+                    if(puls_motor_arm2.getPosition() <= 36) {
+                        wait(1);
+                        while(puls_motor_arm2.getPosition() < 211) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
+
+                            pc.printf("referentie arm 2 %f ", referentie_arm2);
+                            pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                            pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+                        }
+
+                        if(puls_motor_arm2.getPosition() >= 211) {
+                            pc.printf("staat stil\n\r");
+                            state = THUISPOSITIE_RECHTS;
+                        }
+                    }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BTBB: {
+            case BTBB: {                //Geen motoraansturing
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BTBB      ");     //Tekst op LCD scherm
                 pc.printf("BTBB\n\r");              //Controle naar pc sturen
 
                 while(state == BTBB) {
-                    //Motoractie
+                    lcd.printf("   GEEN DOEL    ");
+                    wait(1);
+                    state = WACHT;
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BTBT: {
+            case BTBT: {                //Geen motoraansturing
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BTBT      ");     //Tekst op LCD scherm
                 pc.printf("BTBT\n\r");              //Controle naar pc sturen
 
                 while(state == BTBT) {
-                    //Motoractie
+                    lcd.printf("   GEEN DOEL    ");
+                    wait(1);
+                    state = WACHT;
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BTTB: {
+            case BTTB: {                //Geen motoraansturing
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BTTB      ");     //Tekst op LCD scherm
                 pc.printf("BTTB\n\r");              //Controle naar pc sturen
 
                 while(state == BTTB) {
-                    //Motoractie
+                    lcd.printf("   GEEN DOEL    ");
+                    wait(1);
+                    state = WACHT;
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case BTTT: {
+            case BTTT: {                //Geen motoraansturing
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      BTTT      ");     //Tekst op LCD scherm
                 pc.printf("BTTT\n\r");              //Controle naar pc sturen
 
                 while(state == BTTT) {
-                    //Motoractie
+                    lcd.printf("   GEEN DOEL    ");
+                    wait(1);
+                    state = WACHT;
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TBBB: {
+            case TBBB: {                //Motoraansturing voor doel middenonder
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      TBBB      ");     //Tekst op LCD scherm
                 pc.printf("TBBB\n\r");              //Controle naar pc sturen
 
                 while(state == TBBB) {
-                    //Motoractie
-                }
-                break;                          //Stop met alle acties in deze case
-            }
-
-            case TBBT: {
-                lcd.locate(0,0);                    //Zet tekst op eerste regel
-                lcd.printf("      TBBT      ");     //Tekst op LCD scherm
-                pc.printf("TBBT\n\r");              //Controle naar pc sturen
-
-                while(state == TBBT) {
-                    //Motoractie
-                }
-                break;                          //Stop met alle acties in deze case
-            }
-
-            case TBTB: {
-                lcd.locate(0,0);                    //Zet tekst op eerste regel
-                lcd.printf("      TBTB      ");     //Tekst op LCD scherm
-                pc.printf("TBTB\n\r");              //Controle naar pc sturen
-
-                while(state == TBTB) {
-                    //Motoractie
-                }
-                break;                          //Stop met alle acties in deze case
-            }
-
-            case TBTT: {                        //Motoraansturing voor richten op doel middenboven
-                lcd.locate(0,0);                    //Zet tekst op eerste regel
-                lcd.printf("      TBTT      ");     //Tekst op LCD scherm
-                pc.printf("TBTT\n\r");              //Controle naar pc sturen
-
-                while(state == TBTT) {
-                    //Motoractie
                     while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    //Positie arm 1 is goed
-                    //Snelheid arm 2
-
                     while(puls_motor_arm1.getPosition() > 48) {
                         referentie_arm1 = referentie_arm1 - 132.0/200.0;
                         pc.printf("referentie arm 1 %f ", referentie_arm1);
@@ -1025,57 +1128,169 @@
                     }
                     pc.printf("arm2 op positie");
 
+                    while(puls_motor_arm2.getPosition() > -8) {
 
-                    if(referentie_arm2 >= 167) {
-                        while(puls_motor_arm2.getPosition() > -8) {
+                        referentie_arm2 = -0.5 * 100000 * t * t;
+                        t = t + 0.005;
 
-
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+                        pc.printf("t is %f\n\r", t);
+                    }
 
-                            referentie_arm2 = -0.5 * 200000 * t * t;
-                            t = t + 0.005;
+                    if(puls_motor_arm2.getPosition() <= -8) {
+                        wait(1);
+                        while(puls_motor_arm2.getPosition() < 167) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
 
                             pc.printf("referentie arm 2 %f ", referentie_arm2);
                             pc.printf("get position %d ", puls_motor_arm2.getPosition());
-                            pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
-                            pc.printf("t is %f\n\r", t);
+                            pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
                         }
 
-
-                        if(puls_motor_arm2.getPosition() <= -8) {
-                            wait(1);
-                            while(puls_motor_arm2.getPosition() < 167) {
-                                referentie_arm2 = referentie_arm2 + 167.0/200.0;
-
-
-                                pc.printf("referentie arm 2 %f ", referentie_arm2);
-                                pc.printf("get position %d ", puls_motor_arm2.getPosition());
-                                pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
-                            }
-
-                            if(puls_motor_arm2.getPosition() >= 167) {
-                                pc.printf("staat stil\n\r");
-                                state = WACHT;
-                            }
+                        if(puls_motor_arm2.getPosition() >= 167) {
+                            pc.printf("staat stil\n\r");
+                            state = THUISPOSITIE_MIDDEN;
                         }
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TTBB: {                        //Motoraansturing voor richten op doel linksonder
+            case TBBT: {                //Geen motoraansturing
+                lcd.locate(0,0);                    //Zet tekst op eerste regel
+                lcd.printf("      TBBT      ");     //Tekst op LCD scherm
+                pc.printf("TBBT\n\r");              //Controle naar pc sturen
+
+                while(state == TBBT) {
+                    lcd.printf("   GEEN DOEL    ");
+                    wait(1);
+                    state = WACHT;
+                }
+                break;                          //Stop met alle acties in deze case
+            }
+
+            case TBTB: {                //Motoraansturing voor doel midden
+                lcd.locate(0,0);                    //Zet tekst op eerste regel
+                lcd.printf("      TBTB      ");     //Tekst op LCD scherm
+                pc.printf("TBTB\n\r");              //Controle naar pc sturen
+
+                while(state == TBTB) {
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    while(puls_motor_arm1.getPosition() > 48) {
+                        referentie_arm1 = referentie_arm1 - 132.0/200.0;
+                        pc.printf("referentie arm 1 %f ", referentie_arm1);
+                        pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                        pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
+                    }
+                    pc.printf("arm 1 op positie");
+
+                    while(puls_motor_arm2.getPosition() < 167) {
+                        referentie_arm2 = referentie_arm2 + 44.0/200.0;
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
+                    }
+                    pc.printf("arm2 op positie");
+
+                    while(puls_motor_arm2.getPosition() > -8) {
+
+                        referentie_arm2 = -0.5 * 150000 * t * t;
+                        t = t + 0.005;
+
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+                        pc.printf("t is %f\n\r", t);
+                    }
+
+                    if(puls_motor_arm2.getPosition() <= -8) {
+                        wait(1);
+                        while(puls_motor_arm2.getPosition() < 167) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
+
+                            pc.printf("referentie arm 2 %f ", referentie_arm2);
+                            pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                            pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+                        }
+
+                        if(puls_motor_arm2.getPosition() >= 167) {
+                            pc.printf("staat stil\n\r");
+                            state = THUISPOSITIE_MIDDEN;
+                        }
+                    }
+                }
+                break;                          //Stop met alle acties in deze case
+            }
+
+            case TBTT: {                        //Motoraansturing voor doel middenboven
+                lcd.locate(0,0);                    //Zet tekst op eerste regel
+                lcd.printf("      TBTT      ");     //Tekst op LCD scherm
+                pc.printf("TBTT\n\r");              //Controle naar pc sturen
+
+                while(state == TBTT) {
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    while(puls_motor_arm1.getPosition() > 48) {
+                        referentie_arm1 = referentie_arm1 - 132.0/200.0;
+                        pc.printf("referentie arm 1 %f ", referentie_arm1);
+                        pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                        pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
+                    }
+                    pc.printf("arm 1 op positie");
+
+                    while(puls_motor_arm2.getPosition() < 167) {
+                        referentie_arm2 = referentie_arm2 + 44.0/200.0;
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
+                    }
+                    pc.printf("arm2 op positie");
+
+                    while(puls_motor_arm2.getPosition() > -8) {
+
+                        referentie_arm2 = -0.5 * 200000 * t * t;
+                        t = t + 0.005;
+
+                        pc.printf("referentie arm 2 %f ", referentie_arm2);
+                        pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                        pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+                        pc.printf("t is %f\n\r", t);
+                    }
+
+                    if(puls_motor_arm2.getPosition() <= -8) {
+                        wait(1);
+                        while(puls_motor_arm2.getPosition() < 167) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
+
+                            pc.printf("referentie arm 2 %f ", referentie_arm2);
+                            pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                            pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+                        }
+
+                        if(puls_motor_arm2.getPosition() >= 167) {
+                            pc.printf("staat stil\n\r");
+                            state = THUISPOSITIE_MIDDEN;
+                        }
+                    }
+                }
+                break;                          //Stop met alle acties in deze case
+            }
+
+            case TTBB: {                        //Motoraansturing voor doel linksonder
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      TTBB      ");     //Tekst op LCD scherm
                 pc.printf("TTBB\n\r");              //Controle naar pc sturen
 
                 while(state == TTBB) {
-                    //Motoractie
                     while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    //Positie arm 1 is goed
-                    //Snelheid arm 2
-
-                    referentie_arm2 = 0.5 * 100000 * t * t;
+                    referentie_arm2 = -0.5 * 100000 * t * t;
                     t = t + 0.005;
 
                     pc.printf("referentie arm 2 %f ", referentie_arm2);
@@ -1083,35 +1298,31 @@
                     pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
                     pc.printf("t is %f\n\r", t);
 
-                    if(puls_motor_arm2.getPosition() >= 175) {
+                    if(puls_motor_arm2.getPosition() <= -175) {
                         wait(1);
-                        while(puls_motor_arm2.getPosition() > 0) {
-                            referentie_arm2 = referentie_arm2 - 175.0/200.0;
-
+                        while(puls_motor_arm2.getPosition() < 0) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
 
                             pc.printf("referentie arm 2 %f ", referentie_arm2);
                             pc.printf("get position %d ", puls_motor_arm2.getPosition());
                             pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
                         }
 
-
-                        if(puls_motor_arm2.getPosition() <= 0) {
+                        if(puls_motor_arm2.getPosition() >= 0) {
                             pc.printf("staat stil\n\r");
                             state = WACHT;
                         }
-
                     }
                 }
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TTBT: {                        //Motoraansturing voor terug naar kalibratiepositie
+            case TTBT: {                        //Geen motoraansturing
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      TTBT      ");     //Tekst op LCD scherm
                 pc.printf("TTBT\n\r");              //Controle naar pc sturen
 
                 while(state == TTBT) {
-                    //Motoractie
                     lcd.printf("   GEEN DOEL    ");
                     wait(1);
                     state = WACHT;
@@ -1119,21 +1330,17 @@
                 break;                          //Stop met alle acties in deze case
             }
 
-            case TTTB: {                        //Motoraansturing voor richten op doel linksmidden
+            case TTTB: {                        //Motoraansturing voor doel linksmidden
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      TTTB      ");     //Tekst op LCD scherm
                 pc.printf("TTTB\n\r");              //Controle naar pc sturen
 
                 while(state == TTTB) {
-                    //Motoractie
 
                     while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    //Positie arm 1 is goed
-                    //Snelheid arm 2
-
-                    referentie_arm2 = 0.5 * 150000 * t * t;
+                    referentie_arm2 = -0.5 * 150000 * t * t;
                     t = t + 0.005;
 
                     pc.printf("referentie arm 2 %f ", referentie_arm2);
@@ -1141,31 +1348,27 @@
                     pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
                     pc.printf("t is %f\n\r", t);
 
-                    if(puls_motor_arm2.getPosition() >= 175) {
+                    if(puls_motor_arm2.getPosition() <= -175) {
                         wait(1);
-                        while(puls_motor_arm2.getPosition() > 0) {
-                            referentie_arm2 = referentie_arm2 - 175.0/200.0;
-
+                        while(puls_motor_arm2.getPosition() < 0) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
 
                             pc.printf("referentie arm 2 %f ", referentie_arm2);
                             pc.printf("get position %d ", puls_motor_arm2.getPosition());
                             pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
                         }
 
-
-                        if(puls_motor_arm2.getPosition() <= 0) {
+                        if(puls_motor_arm2.getPosition() >= 0) {
                             pc.printf("staat stil\n\r");
                             state = WACHT;
                         }
-
                     }
                 }
 
                 break;                          //Stop met alle acties in deze case
             }
 
-
-            case TTTT: {                        //Motoraansturing voor richten op doel linksboven
+            case TTTT: {                        //Motoraansturing voor doel linksboven
                 lcd.locate(0,0);                    //Zet tekst op eerste regel
                 lcd.printf("      TTTT      ");     //Tekst op LCD scherm
                 pc.printf("TTBB\n\r");              //Controle naar pc sturen
@@ -1174,10 +1377,7 @@
                     while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
                     regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
 
-                    //Positie arm 1 is goed
-                    //Snelheid arm 2
-
-                    referentie_arm2 = 0.5 * 200000 * t * t;
+                    referentie_arm2 = -0.5 * 200000 * t * t;
                     t = t + 0.005;
 
                     pc.printf("referentie arm 2 %f ", referentie_arm2);
@@ -1185,10 +1385,10 @@
                     pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
                     pc.printf("t is %f\n\r", t);
 
-                    if(puls_motor_arm2.getPosition() >= 175) {
+                    if(puls_motor_arm2.getPosition() <= -175) {
                         wait(1);
-                        while(puls_motor_arm2.getPosition() > 0) {
-                            referentie_arm2 = referentie_arm2 - 175.0/200.0;
+                        while(puls_motor_arm2.getPosition() < 0) {
+                            referentie_arm2 = referentie_arm2 + 175.0/200.0;
 
 
                             pc.printf("referentie arm 2 %f ", referentie_arm2);
@@ -1196,37 +1396,69 @@
                             pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
                         }
 
-
-                        if(puls_motor_arm2.getPosition() <= 0) {
+                        if(puls_motor_arm2.getPosition() >= 0) {
                             pc.printf("staat stil\n\r");
                             state = WACHT;
                         }
-
                     }
                 }
             }
 
             case THUISPOSITIE_MIDDEN: {
                 while(puls_motor_arm2.getPosition() > 123) {
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
                     referentie_arm2 = referentie_arm2 - 44.0/200.0;
+                    pc.printf("referentie arm 2 %f ", referentie_arm2);
+                    pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                    pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
                 }
 
                 while(puls_motor_arm1.getPosition() < 180) {
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
                     referentie_arm1 = referentie_arm1 + 132.0/200.0;
+                    pc.printf("referentie arm 1 %f ", referentie_arm1);
+                    pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                    pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1);
+
                 }
 
                 state = WACHT;
                 break;
             }
 
+            case THUISPOSITIE_RECHTS: {
+                pc.printf("thuispositie rechts");
+                while(puls_motor_arm2.getPosition() > 123) {
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    referentie_arm2 = referentie_arm2 - 88.0/200.0;
+                    pc.printf("referentie arm 2 %f ", referentie_arm2);
+                    pc.printf("get position %d ", puls_motor_arm2.getPosition());
+                    pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+
+                }
+
+                while(puls_motor_arm1.getPosition() < 180) {
+                    while(regelaar_ticker_flag != true);            //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+                    regelaar_ticker_flag = false;                   //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+                    referentie_arm1 = referentie_arm1 + 264.0/200.0;
+                    pc.printf("referentie arm 1 %f ", referentie_arm1);
+                    pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+                    pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1);
+                }
+            }
+
             case WACHT: {
                 lcd.printf("     WACHT      ");
                 wait(5);
                 state = START;
 
-
-
-
                 default: {
                     //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
                     state = RUST;                       //Als dat gebeurt wordt de state rust en begint hij weer vooraan