Revision 33:381bc6635528, committed 2014-11-03

Comitter:

irisl

Date:

Mon Nov 03 22:10:00 2014 +0000

Parent:

32:e35bedc7cefd

Child:

34:7e0bf3e9f135

Commit message:

Dit script leveren we in

Changed in this revision

--- a/main.cpp	Mon Nov 03 20:45:47 2014 +0000
+++ b/main.cpp	Mon Nov 03 22:10:00 2014 +0000
@@ -5,12 +5,11 @@
 #include "encoder.h"
 #include "PwmOut.h"
 
-// define
 #define TSAMP 0.005
-#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 K_P1 (3.5) 
+#define K_I1 (0.01)
+#define K_P2 (3.5) 
+#define K_I2 (0.01) 
 #define I_LIMIT 1.
 #define l_arm 0.5
 
@@ -32,7 +31,7 @@
 //EMG
 AnalogIn    emg0(PTB0); //Analog input
 AnalogIn    emg1(PTC2); //Analog input
-HIDScope scope(4);
+HIDScope scope(3);
 
 //motor1 25D
 Encoder motor1(PTD3,PTD5); //wit, geel
@@ -142,12 +141,12 @@
     average_deltoid(filtered_deltoid, &filtered_average_del);
 
     /*send value to PC. */
-    //scope.set(0,emg_value1);     //Raw EMG signal biceps
+    scope.set(0,emg_value1);     //Raw EMG signal biceps
     //scope.set(1,emg_value2);    //Raw EMG signal Deltoid
-    scope.set(0,filtered_biceps);  //processed float biceps
-    scope.set(1,filtered_average_bi); //processed float deltoid
-    scope.set(2,filtered_deltoid);  //processed float biceps
-    scope.set(3,filtered_average_del); //processed float deltoid
+    scope.set(1,filtered_biceps);  //processed float biceps
+    scope.set(2,filtered_average_bi); //processed float deltoid
+    //scope.set(2,filtered_deltoid);  //processed float biceps
+    //scope.set(3,filtered_average_del); //processed float deltoid
     scope.send();
 
 }
@@ -270,314 +269,324 @@
     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)-0.1) {
+    if(setpoint1 <= -((11.3*2.3*2.0*PI/360)-0.1)) {
         staat1 = 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;
-        }
+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;
+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;
+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;
-        }
+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 = -(150*2.0*PI/360);
+    /*setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+    if(setpoint2 > (160*2.0*PI/360)) { //setpoint in graden
+        setpoint2 = (160.0*2.0*PI/360);
     }
+    if(setpoint2 < -(250.0*2.0*PI/360)) {
+        setpoint2 = -(250.0*2.0*PI/360);
+    }
+    */
+    prev_setpoint2 = setpoint2;
+    if(setpoint2 <= -((150.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;
+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;
+    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;
+    }
+}
+
+// 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);
+    }
+    //if(setpoint2 < -(0.0*2.0*PI/360)) {
+    //   setpoint2 = -(0.0*2.0*PI/360);
+    //}
+    prev_setpoint2 = setpoint2;
+}
+
+// MOTOR aansturing
+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;
+
+                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;
 
-// Motor2 balletje op het laagst slaan
-    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;
+                batje_begin_links ();
+            }
         }
     }
 
-// Motor2 arm terug zetten in beginstand
-    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(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(setpoint2 < -(0.0*2.0*PI/360)) {
-            setpoint2 = -(0.0*2.0*PI/360);
+    }
+    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();
+            }
         }
-        prev_setpoint2 = setpoint2;
+    }
+    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();
+            }
+        }
     }
 
-// MOTOR aansturing
-    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;
-
-                    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();
-                }
-            }
-        }
-
-    }
+}
 
 
 // Hoofdprogramma, hierin staat de aansturing vd LED
-    int main() {
+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);
+    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);
+    // Aanroepen van motoraansturing in motor ticker
+    Ticker looptimer;
+    looptimer.attach(looper_motor,TSAMP);
+
+    while(1) {
 
         while(1) {
-
-            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;
-                        }
+            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;
                     }
-                    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;
-                        }
+                    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;
+                    }
                 }
 
             }
+
         }
-    }
\ No newline at end of file
+    }
+}
\ No newline at end of file