Revision 29:d0dab8921e9d, committed 2014-11-03

Comitter:

jessekaiser

Date:

Mon Nov 03 15:37:48 2014 +0000

Parent:

28:c33a0658605e

Child:

30:7a0a3c272308

Commit message:

wat uitleg bijgeschreven

Changed in this revision

--- a/main.cpp	Sat Nov 01 16:19:53 2014 +0000
+++ b/main.cpp	Mon Nov 03 15:37:48 2014 +0000
@@ -6,12 +6,11 @@
 #include "PwmOut.h"
 
 #define TSAMP 0.005
-#define K_P1 (3.5) //voor motor1 is het 3.5
-#define K_I1 (0.01  *TSAMP) //voor motor1 is het 0.01
-#define K_P2 (0.7)
-#define K_I2 (0.01 *TSAMP)
+#define K_P1 (3.5) //Kp waarde voor motor1, van het batje // 7.0
+#define K_I1 (0.01  *TSAMP) //0.1
+#define K_P2 (0.7) //Kp waarde voor motor2, de arm //10.0
+#define K_I2 (0.01 *TSAMP) //3.0
 #define I_LIMIT 1.
-//#define PI 3.14159265
 #define l_arm 0.5
 
 #define M1_PWM PTC8
@@ -34,7 +33,7 @@
 AnalogIn    emg1(PTC2); //Analog input
 HIDScope scope(4);
 
-//motor 25D
+//motor1 25D
 Encoder motor1(PTD3,PTD5); //wit, geel
 PwmOut pwm_motor1(M2_PWM);
 DigitalOut motordir1(M2_DIR);
@@ -45,8 +44,8 @@
 DigitalOut motordir2(M1_DIR);
 
 // Motor variabelen
-float pwm1_percentage = 0;
-float pwm2_percentage = 0;
+float pwm_out1 = 0;
+float pwm_out2 = 0;
 int cur_pos_motor1;
 int prev_pos_motor1 = 0;
 int cur_pos_motor2;
@@ -87,7 +86,7 @@
 float filtered_average_bi;
 float filtered_average_del;
 
-
+//gemiddelde EMG waarden over 250 sample stappen
 void average_biceps(float filtered_biceps,float *average)
 {
     static float total=0;
@@ -184,14 +183,6 @@
     myledred= 0;
     myledblue =0;
     ledticker.attach(&greenblink,.5);
-    /*  myled1 = 1;
-      myled2 = 1;
-      myled3 = 1;
-      wait(0.1);
-      myled1 = 0;
-      myled2 = 1;
-      myled3 = 1;
-      wait(0.1);*/
 }
 
 void stopblinkgreen()
@@ -228,11 +219,9 @@
 // Motor1 = batje
 // Motor2 = arm
 
-void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
-// maar de locatie van de variabele.
+void clamp(float* in, float min, float max) //Clamp geeft een maximum en minimum limiet aan een functie
 {
-*in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
-    // *in = het getal dat staat op locatie van in --> waarde van new_pwm
+*in > min ? /*(*/*in < max? : *in = max : *in = min;
 }
 
 // PI-regelaar motor1: batje
@@ -273,326 +262,321 @@
 void batje_links ()
 {
     speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden, nu 11.3. 2.3 is de verhouding van de tanden
-        setpoint1 = (11.3*2.3*2.0*PI/360);
-    }
-    if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
-        setpoint1 = -(11.3*2.3*2.0*PI/360);
-    }
-    prev_setpoint1 = setpoint1;
-}
-
-// Motor1 rechts draaien
-void batje_rechts ()
-{
-    speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (11.3*2.3*2.0*PI/360);
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; //bepalen van de setpoint
+    if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //Het eerste getal geeft een aantal graden weer, dus alleen dit hoeft aangepast te worden/
+        setpoint1 = (11.3*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek.
     }
     if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
         setpoint1 = -(11.3*2.3*2.0*PI/360);
     }
-    pwm_motor1.write(abs(pwm1_percentage));
-    prev_setpoint1 = setpoint1;
-    if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
+    if(setpoint1 <= -(11.3*2.3*2.0*PI/360)-0.1) {
         staat1 = 1;
-    }
-}
-
-//Motor1 na links draaien weer terug laten draaien naar beginstand
-void batje_begin_links ()
-{
-    speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (0*2.3*2.0*PI/360);
-    }
-    if(setpoint1 < -(0*2.3*2.0*PI/360)) {
-        setpoint1 = -(0*2.3*2.0*PI/360);
-    }
-    prev_setpoint1 = setpoint1;
-}
-
-//Motor1 na links draaien weer terug laten draaien naar beginstand
-void batje_begin_rechts ()
-{
-    speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (0*2.3*2.0*PI/360);
-    }
-    if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
-        setpoint1 = -(0.0*2.3*2.0*PI/360);
-    }
-    prev_setpoint1 = setpoint1;
-}
-
-// Motor2 balletje op zn hoogst slaan
-void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
-{
-    speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (155.0*2.0*PI/360);
-    }
-    if(setpoint2 < -(155.0*2.0*PI/360)) {
-        setpoint2 = -(155.0*2.0*PI/360);
-    }
-    prev_setpoint2 = setpoint2;
-    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
-        staat2 = 1;
-    }
-}
-
-// Motor2 balletje in het midden slaan
-void arm_mid ()
-{
-    speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (155.0*2.0*PI/360);
-    }
-    if(setpoint2 < -(155.0*2.0*PI/360)) {
-        setpoint2 = -(155.0*2.0*PI/360);
-    }
-    prev_setpoint2 = setpoint2;
-    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
-        staat2 = 1;
+        prev_setpoint1 = setpoint1;
     }
 }
+// Motor1 rechts draaien
+    void batje_rechts () {
+        speed1_rad = 1.0;
+        setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+        if(setpoint1 > (11.3*2.3*2.0*PI/360)) {
+            setpoint1 = (11.3*2.3*2.0*PI/360);
+        }
+        if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
+            setpoint1 = -(11.3*2.3*2.0*PI/360);
+        }
+        prev_setpoint1 = setpoint1;
+        if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
+            staat1 = 1;
+        }
+    }
+
+//Motor1 na links draaien weer terug laten draaien naar beginstand
+    void batje_begin_links () {
+        speed1_rad = 1.0;
+        setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+        if(setpoint1 > (0*2.3*2.0*PI/360)) {
+            setpoint1 = (0*2.3*2.0*PI/360);
+        }
+        if(setpoint1 < -(0*2.3*2.0*PI/360)) {
+            setpoint1 = -(0*2.3*2.0*PI/360);
+        }
+        prev_setpoint1 = setpoint1;
+    }
+
+//Motor1 na links draaien weer terug laten draaien naar beginstand
+    void batje_begin_rechts () {
+        speed1_rad = -1.0;
+        setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+        if(setpoint1 > (0*2.3*2.0*PI/360)) {
+            setpoint1 = (0*2.3*2.0*PI/360);
+        }
+        if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
+            setpoint1 = -(0.0*2.3*2.0*PI/360);
+        }
+        prev_setpoint1 = setpoint1;
+    }
+
+// Motor2 balletje op zn hoogst slaan
+    void arm_hoog () {
+        speed2_rad = 6.0;
+        setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+        if(setpoint2 > (155.0*2.0*PI/360)) {
+            setpoint2 = (155.0*2.0*PI/360);
+        }
+        if(setpoint2 < -(155.0*2.0*PI/360)) {
+            setpoint2 = -(155.0*2.0*PI/360);
+        }
+        prev_setpoint2 = setpoint2;
+        if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+            staat2 = 1;
+        }
+    }
+
+// Motor2 balletje in het midden slaan
+    void arm_mid () {
+        speed2_rad = 4.0;
+        setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+        if(setpoint2 > (155.0*2.0*PI/360)) {
+            setpoint2 = (155.0*2.0*PI/360);
+        }
+        if(setpoint2 < -(155.0*2.0*PI/360)) {
+            setpoint2 = -(155.0*2.0*PI/360);
+        }
+        prev_setpoint2 = setpoint2;
+        if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+            staat2 = 1;
+        }
+    }
 
 // Motor2 balletje op het laagst slaan
-void arm_laag ()
-{
-    speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (155*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (155*2.0*PI/360);
+    void arm_laag () {
+        speed2_rad = 2.0;
+        setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+        if(setpoint2 > (155*2.0*PI/360)) {
+            setpoint2 = (155*2.0*PI/360);
+        }
+        if(setpoint2 < -(155.0*2.0*PI/360)) {
+            setpoint2 = -(155.0*2.0*PI/360);
+        }
+        prev_setpoint2 = setpoint2;
+        if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+            staat2 = 1;
+        }
     }
-    if(setpoint2 < -(155.0*2.0*PI/360)) {
-        setpoint2 = -(155.0*2.0*PI/360);
-    }
-    prev_setpoint2 = setpoint2;
-    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
-        staat2 = 1;
-    }
-}
 
 // Motor2 arm terug zetten in beginstand
-void arm_begin ()
-{
-    speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (0.0*2.0*PI/360);
+    void arm_begin () {
+        speed2_rad = 1.0;
+        setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+        if(setpoint2 > (0.0*2.0*PI/360)) {
+            setpoint2 = (0.0*2.0*PI/360);
+        }
+        if(setpoint2 < -(0.0*2.0*PI/360)) {
+            setpoint2 = -(0.0*2.0*PI/360);
+        }
+        prev_setpoint2 = setpoint2;
     }
-    if(setpoint2 < -(0.0*2.0*PI/360)) {
-        setpoint2 = -(0.0*2.0*PI/360);
-    }
-    prev_setpoint2 = setpoint2;
-}
 
 // MOTOR aansturing
-void looper_motor()
-{
-    //MOTOR1
-    pc.printf("%d \r\n", motor1.getPosition());
-    cur_pos_motor1 = motor1.getPosition();
-    pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
-    pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
-    if (pwm1_percentage < -1.0) {
-        pwm1_percentage = -1.0;
-    }
-    if (pwm1_percentage > 1.0) {
-        pwm1_percentage = 1.0;
-    }
-    pwm_motor1.write(abs(pwm1_percentage));
-    if(pwm1_percentage > 0) {
-        motordir1 = 0;
-    } else {
-        motordir1 = 1;
-    }
+    void looper_motor() {
+        pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition()); //Geeft de posities weer van beide motoren met een sample frequentie van 0.005
+
+        //MOTOR1
+        \
+        cur_pos_motor1 = motor1.getPosition();
+        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //voor 1 rotatie van de motoras geldt 24(aantal cpr vd encoder)*172(gearbox ratio)=4128 counts.
+        pwm_out1 = pid1(setpoint1, pos_motor1_rad);
+        if (pwm_out1 < -1.0) { //Hier wordt de grens voor de pwm waarde ingesteld.
+            pwm_out1 = -1.0;
+        }
+        if (pwm_out1 > 1.0) {
+            pwm_out1 = 1.0;
+        }
+        pwm_motor1.write(abs(pwm_out1));
+        if(pwm_out1 > 0) {
+            motordir1 = 0;
+        } else {
+            motordir1 = 1;
+        }
+
+        //MOTOR2
+        cur_pos_motor2 = motor2.getPosition();
+        pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
+        pwm_out2 = pid2(setpoint2, pos_motor2_rad); //
+        if (pwm_out2 < -1.0) {
+            pwm_out2 = -1.0;
+        }
+        if (pwm_out2 > 1.0) {
+            pwm_out2 = 1.0;
+        }
+        pwm_motor2.write(abs(pwm_out2));
+        if(pwm_out2 > 0) {
+            motordir2 = 0;
+        } else {
+            motordir2 = 1;
+        }
+
+
+        //STATES
+
+        //Het batje draait naar opgegeven positie, doet dan een bepaalde tijd niks (wait_iterator), en draait daarna weer terug
+        if (batje_hoek == 1) {
+            if(staat1 == 0) {
+                batje_rechts();
+                wait_iterator1 = 0;
+            } else if(staat1 ==1) {
+                wait_iterator1++;
+                if(wait_iterator1 > 1200) {
+                    staat1 = 2;
 
-    //MOTOR2
-    cur_pos_motor2 = motor2.getPosition();
-    pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
-    pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
-    if (pwm2_percentage < -1.0) {
-        pwm2_percentage = -1.0;
-    }
-    if (pwm2_percentage > 1.0) {
-        pwm2_percentage = 1.0;
-    }
-    pwm_motor2.write(abs(pwm2_percentage));
-    if(pwm2_percentage > 0) {
-        motordir2 = 0;
-    } else {
-        motordir2 = 1;
+                    batje_begin_rechts();
+                }
+            }
+        }
+        if (batje_hoek == 2) {
+            if(staat1 == 0) {
+                batje_links();
+                wait_iterator1 = 0;
+            } else if(staat1 ==1) {
+                wait_iterator1++;
+                if(wait_iterator1 > 1200) {
+                    staat1 = 2;
+
+                    batje_begin_links ();
+                }
+            }
+        }
+
+        if(arm_hoogte == 1) {
+            if(staat2 == 0) {
+                arm_laag();
+                wait_iterator2 = 0;
+            } else if(staat2 == 1) {
+                wait_iterator2++;
+                if(wait_iterator2 > 400) {
+                    staat2 = 2;
+
+                    arm_begin();
+                }
+            }
+        }
+        if(arm_hoogte == 2) {
+            if(staat2 == 0) {
+                arm_mid();
+                wait_iterator2 = 0;
+            } else if(staat2 == 1) {
+                wait_iterator2++;
+                if(wait_iterator2 > 400) {
+                    staat2 = 2;
+
+                    arm_begin();
+                }
+            }
+        }
+        if(arm_hoogte == 3) {
+            if(staat2 == 0) {
+                arm_hoog();
+                wait_iterator2 = 0;
+            } else if(staat2 == 1) {
+                wait_iterator2++;
+                if(wait_iterator2 > 400) {
+                    staat2 = 2;
+
+                    arm_begin();
+                }
+            }
+        }
+
     }
 
 
-    //STATES
-
-    if (batje_hoek == 1) {
-        if(staat1 == 0) {
-            batje_rechts();
-            wait_iterator1 = 0;
-        } else if(staat1 ==1) {
-            wait_iterator1++;
-            if(wait_iterator1 > 400) {
-                staat1 = 2;
-
-                batje_begin_rechts();
-            }
-        }
-    }
-    if (batje_hoek == 2) {
-        if(staat1 == 0) {
-            batje_links();
-            wait_iterator1 = 0;
-        } else if(staat1 ==1) {
-            wait_iterator1++;
-            if(wait_iterator1 > 400) {
-                staat1 = 2;
-
-                batje_begin_links ();
-            }
-        }
-    }
-
-    if(arm_hoogte == 1) {
-        if(staat2 == 0) {
-            arm_laag();
-            wait_iterator2 = 0;
-        } else if(staat2 == 1) {
-            wait_iterator2++;
-            if(wait_iterator2 > 400) {
-                staat2 = 2;
+// Hoofdprogramma, hierin staat de aansturing vd LED
+    int main() {
 
-                arm_begin();
-            }
-        }
-    }
-    if(arm_hoogte == 2) {
-        if(staat2 == 0) {
-            arm_mid();
-            wait_iterator2 = 0;
-        } else if(staat2 == 1) {
-            wait_iterator2++;
-            if(wait_iterator2 > 400) {
-                staat2 = 2;
-
-                arm_begin();
-            }
-        }
-    }
-    if(arm_hoogte == 3) {
-        if(staat2 == 0) {
-            arm_hoog();
-            wait_iterator2 = 0;
-        } else if(staat2 == 1) {
-            wait_iterator2++;
-            if(wait_iterator2 > 400) {
-                staat2 = 2;
+        pwm_motor1.period_us(100);
+        motor1.setPosition(0);
+        pwm_motor2.period_us(100);
+        motor2.setPosition(0);
+        pc.baud(115200);
+        // Ticker EMG signaal meten
+        Ticker log_timer;
+        //set up filters. Use external array for constants
+        arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
+        arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
+        arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
+        arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
+        // Uitvoeren van ticker EMG, sample frequentie 500Hz
+        log_timer.attach(looper, 0.002);
 
-                arm_begin();
-            }
-        }
-    }
-
-}
-
-
-// Hoofdprogramma, hierin staat de aansturing vd LED
-int main()
-{
-
-    pwm_motor1.period_us(100);
-    motor1.setPosition(0);
-    pwm_motor2.period_us(100);
-    motor2.setPosition(0);
-    pc.baud(115200);
-    // Ticker EMG signaal meten
-    Ticker log_timer;
-    //set up filters. Use external array for constants
-    arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
-    arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
-    arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
-    arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
-    // Uitvoeren van ticker EMG, sample frequentie 500Hz
-    log_timer.attach(looper, 0.002);
-
-    // Aanroepen van motoraansturing in motor ticker
-    Ticker looptimer;
-    looptimer.attach(looper_motor,TSAMP);
-
-    while(1) {
+        // Aanroepen van motoraansturing in motor ticker
+        Ticker looptimer;
+        looptimer.attach(looper_motor,TSAMP);
 
         while(1) {
-            pc.printf("Span de biceps aan om het instellen te starten.\n");
-            do {
-                ShineRed();
-            } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
-            if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen
-                BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
-                BlinkGreen(); // groen knipperen, meten van spieraanspanning
-                while (1) { // eerste loop, keuze voor de positie van het batje
-                    pc.printf("In de loop.\n");
-                    if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden
-                        stopblinkgreen();
-                        pc.printf("ShineGreen.\n");
-                        ShineGreen();
-                        wait (4);
-                        break;
+
+            while(1) {
+                pc.printf("Span de biceps aan om het instellen te starten.\n");
+                do {
+                    ShineRed();
+                } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
+                if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen
+                    BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
+                    BlinkGreen(); // groen knipperen, meten van spieraanspanning
+                    while (1) { // eerste loop, keuze voor de positie van het batje
+                        pc.printf("In de loop.\n");
+                        if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden
+                            stopblinkgreen();
+                            pc.printf("ShineGreen.\n");
+                            ShineGreen();
+                            wait (4);
+                            break;
+                        }
+                        if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links
+                            stopblinkgreen();
+                            pc.printf("ShineBlue.\n");
+                            ShineBlue();
+                            batje_hoek = 2;
+                            wait(4);
+                            break;
+                        } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
+                            stopblinkgreen();
+                            pc.printf("ShineRed.\n");
+                            ShineRed();
+                            batje_hoek = 1;
+                            wait (4);
+                            break;
+                        }
                     }
-                    if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links
-                        stopblinkgreen();
-                        pc.printf("ShineBlue.\n");
-                        ShineBlue();
-                        batje_hoek = 2;
-                        wait(4);
-                        break;
-                    } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
-                        stopblinkgreen();
-                        pc.printf("ShineRed.\n");
-                        ShineRed();
-                        batje_hoek = 1;
-                        wait (4);
-                        break;
+                    BlinkGreen();
+                    while (1) { // loop voor het instellen van de kracht
+                        pc.printf("In de loop.\n");
+                        if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
+                            stopblinkgreen();
+                            pc.printf("ShineGreen.\n");
+                            ShineGreen();
+                            arm_hoogte = 3;
+                            wait (4);
+                            break;
+                        }
+                        if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan
+                            stopblinkgreen();
+                            pc.printf("ShineBlue.\n");
+                            ShineBlue();
+                            arm_hoogte = 1;
+                            wait(4);
+                            break;
+                        } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
+                            stopblinkgreen();
+                            pc.printf("ShineRed.\n");
+                            ShineRed();
+                            arm_hoogte = 2;
+                            wait (4);
+                            break;
+                        }
                     }
-                }
-                BlinkGreen();
-                while (1) { // loop voor het instellen van de kracht
-                    pc.printf("In de loop.\n");
-                    if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
-                        stopblinkgreen();
-                        pc.printf("ShineGreen.\n");
-                        ShineGreen();
-                        arm_hoogte = 3;
-                        wait (4);
-                        break;
-                    }
-                    if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan
-                        stopblinkgreen();
-                        pc.printf("ShineBlue.\n");
-                        ShineBlue();
-                        arm_hoogte = 1;
-                        wait(4);
-                        break;
-                    } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
-                        stopblinkgreen();
-                        pc.printf("ShineRed.\n");
-                        ShineRed();
-                        arm_hoogte = 2;
-                        wait (4);
-                        break;
-                    }
+
                 }
 
             }
-
         }
-    }
-}
\ No newline at end of file
+    }
\ No newline at end of file