Diff: main.cpp

Revision:

11:687aa4656a6e

Parent:

10:cd89569cd847

Child:

12:3fec73bc3318

diff -r cd89569cd847 -r 687aa4656a6e main.cpp
--- a/main.cpp	Mon Nov 03 12:34:33 2014 +0000
+++ b/main.cpp	Mon Nov 03 14:46:31 2014 +0000
@@ -16,6 +16,7 @@
 #include "HIDScope.h"
 #include "PwmOut.h"
 #include "arm_math.h"
+#include "MODSERIAL.h"
 
 /*definieren pinnen Motoren*/
 #define M1_PWM PTA5
@@ -35,14 +36,14 @@
 */
 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
 Encoder motor1(PTD3,PTD1);
-Encoder motor2(PTD5, PTD0); 
+Encoder motor2(PTD5, PTD0);
 PwmOut pwm_motor1(M1_PWM);
-PwmOut pwm_motor2(M2_PWM); 
+PwmOut pwm_motor2(M2_PWM);
 DigitalOut motordir1(M1_DIR);
-DigitalOut motordir2(M2_DIR); 
+DigitalOut motordir2(M2_DIR);
 DigitalOut LEDGREEN(LED_GREEN);
-DigitalOut LEDRED(LED_RED); 
-Serial pc(USBTX,USBRX);
+DigitalOut LEDRED(LED_RED);
+MODSERIAL pc(USBTX,USBRX);
 HIDScope scope(3);
 AnalogIn emg(PTB1);
 /*
@@ -56,11 +57,11 @@
 arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
 int previous_herhalingen = 0;
 int current_herhalingen = 0;
-int current_herhalingen_1 = 0; 
-int previous_herhalingen_1 = 0; 
+int current_herhalingen_1 = 0;
+int previous_herhalingen_1 = 0;
 int previous_pos_motor1 = 0;
 int current_pos_motor1;
-int current_pos_motor2; 
+int current_pos_motor2;
 int EMG = 1;
 int aantal_pieken = 0;
 int doel;
@@ -74,7 +75,7 @@
 float pid(float setpoint, float measurement);
 float pos_motor1_rad;
 float PWM1_percentage = 0;
-float PWM1; 
+float PWM1;
 float PWM2;
 float setpoint = 0;
 float prev_setpoint = 0;
@@ -90,7 +91,7 @@
 float emg_max = 0;
 float emg_treshhold_laag = 0;
 float emg_treshhold_hoog = 0;
-float marge = 0; 
+float marge = 0;
 float PWM_richting1;
 float PWM_richting2;
 float PWM_richting3;
@@ -98,30 +99,32 @@
 enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
 state_t state = RUST;
 
-void rust() {
-    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+void rust()
+{
+    current_herhalingen = previous_herhalingen + 1;
+    previous_herhalingen = current_herhalingen;
 }//void rust
-    
-void pieken_tellen(){
-    if (emg_filtered>=emg_treshhold_hoog) 
-    {
+
+void pieken_tellen()
+{
+    if (emg_filtered>=emg_treshhold_hoog) {
         aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
     }//if
-    if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt
-    {
+    if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt
         aanspan=false;
         aantal_pieken++;
         doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;
-        
+
     }//if
 }//void pieken_tellen
 
-void emg_filtering() {
+void emg_filtering()
+{
     uint16_t emg_value;
     float emg_value_f32;
     emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
     emg_value_f32 = emg.read();
-    
+
     arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
     arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
     arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
@@ -129,84 +132,92 @@
     arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
     //scope.set(0,emg_value);     //uint value
     //scope.set(1,emg_filtered);  //processed float
-    if(state!=EMG_KALIBRATIE)
-    {
+    if(state!=EMG_KALIBRATIE) {
         pieken_tellen();
     }//if
     //pc.printf("%d\n\r",doel);
-    scope.set(0, doel); 
+    scope.set(0, doel);
     scope.set(1, aantal_pieken);
     scope.send();
 }//void emg_filtering()
 
-void emg_max_meting(){
+void emg_max_meting()
+{
     emg_filtering();
-    if (emg_filtered>=emg_max)
-    {
+    if (emg_filtered>=emg_max) {
         emg_max=emg_filtered;
     }//if
-    emg_treshhold_laag = 0.3*emg_max;
-    emg_treshhold_hoog = 0.6*emg_max;
+    emg_treshhold_laag = 0.4*emg_max;
+    emg_treshhold_hoog = 0.7*emg_max;
 }//void emg_max_meting
 
-void akkoord_geven(){
+void akkoord_geven()
+{
     emg_filtering();
 }
 
-void emg_kalibratie() {
+void emg_kalibratie()
+{
     //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het  nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
-        current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+    current_herhalingen = previous_herhalingen + 1;
+    previous_herhalingen = current_herhalingen;
     //}
-    if(current_herhalingen<=1000)
-    {
+    if(current_herhalingen<=1000) {
         emg_max_meting();
     }//if
 }//void emg_kalibratie
 
-void arm_kalibratie() {
+void arm_kalibratie()
+{
     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
     motor1.setPosition(0);
-    motor2.setPosition(0);  
+    motor2.setPosition(0);
     pwm_motor1.period_us(100);
     pwm_motor2.period_us(100);
-    akkoord_geven();  
+    akkoord_geven();
 }//void arm_kalibratie
 
-void doel_bepaling() {
-    if(200<=current_herhalingen && current_herhalingen <1200){
+void doel_bepaling()
+{
+    if(200<=current_herhalingen && current_herhalingen <1200) {
         emg_filtering();
         doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
     }//if
-    else if(current_herhalingen == 1200 && state==METEN_HOOGTE){
+    else if(current_herhalingen == 1200 && state==METEN_HOOGTE) {
         doel_hoogte = doel;
         aantal_pieken = 0;
-        doel = 0; 
-    }
-    else if(current_herhalingen == 1200 && state==METEN_RICHTING){
+        doel = 0;
+    } else if(current_herhalingen == 1200 && state==METEN_RICHTING) {
         doel_richting = doel;
         aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
         doel = 0;
-    }
-    else if(1200<current_herhalingen && current_herhalingen<=2200){
-        akkoord_geven();        
+    } else if(1200<current_herhalingen && current_herhalingen<=2200) {
+        akkoord_geven();
     }//else if
-    else{
-    }//else    
+    else {
+    }//else
 }//void doel_bepaling
 
-void meten_hoogte() {
-    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+void meten_hoogte()
+{
+    current_herhalingen = previous_herhalingen + 1;
+    previous_herhalingen = current_herhalingen;
     doel_bepaling();
 }//void meten_hoogte
 
-void meten_richting() {
-    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+void meten_richting()
+{
+    current_herhalingen = previous_herhalingen + 1;
+    previous_herhalingen = current_herhalingen;
     doel_bepaling();
 }//void meten_richting
 
-void instellen_richting() {
+void instellen_richting()
+{
     current_pos_motor2 = motor2.getPosition();
-if (doel_richting ==1){
+    pc.printf("%d\n\r", current_pos_motor2);
+    /*
+    if (doel_richting ==1){
     if (state==RETURN2RUST){
         motordir2 = 1;
         while (current_pos_motor2 > 0){
@@ -221,10 +232,10 @@
         }//while (current_pos_motor2 < rad_richting1)
         current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//else
-}//if (doel_richting ==1)
+    }//if (doel_richting ==1)
 
 
-else if (doel_richting == 2){
+    else if (doel_richting == 2){
     if (state==RETURN2RUST){
         motordir2 = 1;
         while (current_pos_motor2 > 0){
@@ -239,9 +250,9 @@
         }//while (current_pos_motor2 < rad_richting1)
         current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
-}//if (doel_richting ==1)
+    }//if (doel_richting ==1)
 
-else if(doel_richting == 3){
+    else if(doel_richting == 3){
     if (state==RETURN2RUST){
         motordir2 =1;
         while (current_pos_motor2 > 0){
@@ -256,55 +267,50 @@
         }//while (current_pos_motor2 < rad_richting1)
         current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
     }//if (doel_richting == 1)
-}//if (doel_richting ==1)
+    }//if (doel_richting ==1)
+    */
 }//void instellen_richting
 
-void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge){
-    
+void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge)
+{
+
     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
-    
+
     //nu gaan we snelheid volgen d.m.v. positie regeling
-    if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) //if position error < ...rad, then stop.
-    {
-        speed_radpersecond = 0; 
+    if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) { //if position error < ...rad, then stop.
+        speed_radpersecond = 0;
         setpoint = pos_motor1_rad;
-        current_herhalingen = previous_herhalingen + 1; 
+        current_herhalingen = previous_herhalingen + 1;
         previous_herhalingen = current_herhalingen;
-        pc.printf("stop\n\r");
+        //pc.printf("stop\n\r");
         PWM1_percentage = 0;
     }//if
-    else if(pos_motor1_rad - position_setpoint_rad < 0)
-    {    
+    else if(pos_motor1_rad - position_setpoint_rad < 0) {
         setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
-        PWM1_percentage = pid(setpoint, pos_motor1_rad);       
+        PWM1_percentage = pid(setpoint, pos_motor1_rad);
     }//else if
-    else
-    {
+    else {
         setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
-        PWM1_percentage = pid(setpoint, pos_motor1_rad);       
+        PWM1_percentage = pid(setpoint, pos_motor1_rad);
     }//else
-    pc.printf("%f\n\r",PWM1_percentage);
-    
-    if (PWM1_percentage < -100)
-    {
+    //pc.printf("%f\n\r",PWM1_percentage);
+
+    if (PWM1_percentage < -100) {
         PWM1_percentage = -100;
     }//if
-    else if (PWM1_percentage >100)
-    {
+    else if (PWM1_percentage >100) {
         PWM1_percentage =100;
     }//else if
-    
-    if(PWM1_percentage < 0)
-    {
+
+    if(PWM1_percentage < 0) {
         motordir1 = 1;
     }//if
-    else
-    {
+    else {
         motordir1 = 0;
     }//else
-        
+
     pwm_motor1.write(abs(PWM1_percentage/100.));
     prev_setpoint = setpoint;
 }//void GotoPosition
@@ -317,7 +323,7 @@
     static float    out_i = 0;
     float           out_d = 0;
     error  = (setpoint-measurement);
-    out_p  = error*K_P; 
+    out_p  = error*K_P;
     out_i += error*K_I;
     out_d  = (error-prev_error)*K_D;
     clamp(&out_i,-I_LIMIT,I_LIMIT);
@@ -327,148 +333,133 @@
 
 void clamp(float* in, float min, float max)
 {
-    *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; 
+*in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
 }//void clamp
-    
+
 void statemachinefunction()
 {
+    pc.printf(".");
     switch(state) {
         case RUST: {
             rust();
             /*voorwaarde wanneer hij door kan naar de volgende case*/
-            if (current_herhalingen == 100 && EMG == 1) 
-            {
+            if (current_herhalingen == 100 && EMG == 1) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 state = EMG_KALIBRATIE;
                 EMG = 0; //door EMG op 0 te zetten word deze loop nooit meer doorlopen, daarna zal altijd else worden uitgevoerd. Wat ook gelijk het kalibreren van de arm overslaat. Men kan na 1 keer kalibreren dus vaker achter elkaar schieten
-            }//if (current_herhalingen == 100 && EMG == 1) 
-            else if(current_herhalingen == 100)
-            {
-                current_herhalingen = 0; 
-                previous_herhalingen = 0; 
-                state = METEN_HOOGTE; 
+            }//if (current_herhalingen == 100 && EMG == 1)
+            else if(current_herhalingen == 100) {
+                current_herhalingen = 0;
+                previous_herhalingen = 0;
+                state = METEN_HOOGTE;
             }//else
             break;
-        }//case RUST: 
+        }//case RUST:
 
-        case EMG_KALIBRATIE: 
-        {
+        case EMG_KALIBRATIE: {
             emg_kalibratie();
-            if (current_herhalingen >=1000) /*waarom >= en niet ==?*/
-            {
+            if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
                 state = ARM_KALIBRATIE;
-            }//if (current_herhalingen >=1000) 
+            }//if (current_herhalingen >=1000)
             break;
         }//case EMG_KALIBRATIE
-        
-        case ARM_KALIBRATIE: 
-        {
+
+        case ARM_KALIBRATIE: {
             arm_kalibratie();
-            if (aantal_pieken == 1) 
-            {
-                current_herhalingen = 0;
-                previous_herhalingen = 0;
-                aantal_pieken = 0; 
-                doel = 0; 
-                state = METEN_HOOGTE;
-            }//if (current_herhalingen == 100) 
-            break;
-        }//case ARM_KALIBRATIE:
-
-        case METEN_HOOGTE: 
-        {
-            meten_hoogte();
-            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) 
-            {
+            if (aantal_pieken == 1) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
-                //doel_hoogte = 0; 
-                state = METEN_RICHTING;
-            }//if (current_herhalingen == 2800 && aantal_pieken == 1) 
-            else if (current_herhalingen == 2200)
-            {
+                state = METEN_HOOGTE;
+            }//if (current_herhalingen == 100)
+            break;
+        }//case ARM_KALIBRATIE:
+
+        case METEN_HOOGTE: {
+            meten_hoogte();
+            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
-                doel = 0; 
+                doel = 0;
+                //doel_hoogte = 0;
+                state = METEN_RICHTING;
+            }//if (current_herhalingen == 2800 && aantal_pieken == 1)
+            else if (current_herhalingen == 2200) {
+                current_herhalingen = 0;
+                previous_herhalingen = 0;
+                aantal_pieken = 0;
+                doel = 0;
                 state = METEN_HOOGTE;
             }///else
             break;
         }//case METEN_HOOGTE
 
-        case METEN_RICHTING: 
-        {
+        case METEN_RICHTING: {
             meten_richting();
-            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) 
-            {
+            if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
                 doel = 0;
                 state = INSTELLEN_RICHTING;
-            }//if (current_herhalingen == 2800 && aantal_pieken == 1) 
-            else if (current_herhalingen == 2200)
-            {
+            }//if (current_herhalingen == 2800 && aantal_pieken == 1)
+            else if (current_herhalingen == 2200) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 aantal_pieken = 0;
-                doel = 0; 
+                doel = 0;
                 state = METEN_RICHTING;
             }///else
             break;
         }//case METEN_RICHTING
 
-        case INSTELLEN_RICHTING: 
-        {
+        case INSTELLEN_RICHTING: {
             instellen_richting();
-            if (current_herhalingen == 100) 
+            /*if (current_herhalingen == 100)
             {
                 current_herhalingen_1 = 0;
                 previous_herhalingen_1 = 0;
                 doel_richting = 0;
                 state = SLAAN;
             }//if (current_herhalingen == 100)
-        break;  
+            */
+            break;
         }//case INSTELLEN_RICHTING
 
-        case SLAAN: 
-        {
+        case SLAAN: {
             GotoPosition(1.5 ,8, 0.1);
-            if (current_herhalingen == 400) 
-            {
+            if (current_herhalingen == 400) {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                prev_setpoint =0; 
+                prev_setpoint =0;
                 setpoint =0;
                 state = RETURN2RUST;
-            }//if (current_herhalingen == 100) 
-        break;    
+            }//if (current_herhalingen == 100)
+            break;
         }//case SLAAN
 
-        case RETURN2RUST: 
-        {
+        case RETURN2RUST: {
             instellen_richting();
             GotoPosition(0,4, 0.05);
             doel_richting = 0;
-            doel_hoogte = 0; 
-            if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) 
-            {
+            doel_hoogte = 0;
+            if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) {
                 state = RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                current_herhalingen = 0; 
+                current_herhalingen = 0;
                 current_herhalingen = 0;
-            }//if (current_herhalingen == 100) 
+            }//if (current_herhalingen == 100)
             break;
         }// case RETURN2RUST
-        
+
         default: {
             state = RUST;
         }//default
@@ -477,172 +468,177 @@
 }//void statemachinefunction
 
 
-void screenupdate(){
-    if(state==RUST){
-        lcd.cls(); 
+void screenupdate()
+{
+    pc.printf("l");
+    if(state==RUST) {
+        lcd.cls();
         lcd.locate(0,0);
         lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
         lcd.locate(0,1);
         lcd.printf("  GROEP 7   ");
     }//if(state==RUST)
-    
-    else if(state==EMG_KALIBRATIE){
+
+    else if(state==EMG_KALIBRATIE) {
         lcd.cls();
         lcd.locate(0,0);
         lcd.printf("Max. aanspannen");
-        if(current_herhalingen<=200){
-            lcd.locate(0,1); 
+        if(current_herhalingen<=200) {
+            lcd.locate(0,1);
             lcd.printf("nog 5 sec.");
         }//if(current_herhalingen<=200)
-        else if(current_herhalingen<=400){
-            lcd.locate(0,1); 
+        else if(current_herhalingen<=400) {
+            lcd.locate(0,1);
             lcd.printf("nog 4 sec.");
         }//else if(current_herhalingen<=400)
-        else if(current_herhalingen<=600){
-            lcd.locate(0,1); 
+        else if(current_herhalingen<=600) {
+            lcd.locate(0,1);
             lcd.printf("nog 3 sec.");
         }//else if(current_herhalingen<=600)
-        else if(current_herhalingen<=800){
-            lcd.locate(0,1); 
+        else if(current_herhalingen<=800) {
+            lcd.locate(0,1);
             lcd.printf("nog 2 sec.");
         }//else if(current_herhalingen<=800)
-        else if(current_herhalingen<=1000){
-            lcd.locate(0,1); 
+        else if(current_herhalingen<=1000) {
+            lcd.locate(0,1);
             lcd.printf("nog 1 sec.");
         }//else if(current_herhalingen<=1000)
     }//else if(state==EMG_KALIBRATIE)
-    
-    else if(state==ARM_KALIBRATIE){
+
+    else if(state==ARM_KALIBRATIE) {
         lcd.cls();
         lcd.locate(0,0);
         lcd.printf("Set arm to zero");
-        lcd.locate(0,1); 
+        lcd.locate(0,1);
         lcd.printf("Klaar? Span aan");
     }//else if(state==ARM_KALIBRATIE)
-    
-    else if(state==METEN_HOOGTE){
+
+    else if(state==METEN_HOOGTE) {
         lcd.cls();
-        if(current_herhalingen<=200){
+        if(current_herhalingen<=200) {
             lcd.locate(0,0);
             lcd.printf("Hoogte bepalen:");
             lcd.locate(0,1);
             lcd.printf("span aan per vak");
         }//if(current_herhalingen<=200){
-        else if(200<=current_herhalingen && current_herhalingen<1200){
+        else if(200<=current_herhalingen && current_herhalingen<1200) {
             lcd.locate(0,0);
             lcd.printf("Vak %d",doel);
-            if(current_herhalingen<=400){
-                lcd.locate(0,1); 
+            if(current_herhalingen<=400) {
+                lcd.locate(0,1);
                 lcd.printf("nog 5 sec.");
             }//if(current_herhalingen<=400)
-            else if(current_herhalingen<=600){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<=600) {
+                lcd.locate(0,1);
                 lcd.printf("nog 4 sec.");
             }//else if(current_herhalingen<=600)
-            else if(current_herhalingen<=800){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<=800) {
+                lcd.locate(0,1);
                 lcd.printf("nog 3 sec.");
             }//else if(current_herhalingen<=800)
-            else if(current_herhalingen<=1000){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<=1000) {
+                lcd.locate(0,1);
                 lcd.printf("nog 2 sec.");
             }//else if(current_herhalingen<=1000)
-            else if(current_herhalingen<1200){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<1200) {
+                lcd.locate(0,1);
                 lcd.printf("nog 1 sec.");
             }//else if(current_herhalingen<=1200)
         }//else if(200<=current_herhalingen<=1200)
-        else if(current_herhalingen<=2200){
+        else if(current_herhalingen<=2200) {
             lcd.locate(0,0);
             lcd.printf("Vak %d akkoord?",doel_hoogte);
             lcd.locate(0,1);
             lcd.printf("Span aan");
         }//else if(current_herhalingen<=1600){
-        else{
+        else {
             lcd.locate(0,0);
             lcd.printf("Opnieuw hoogte");
             lcd.locate(0,1);
             lcd.printf("bepalen");
         }//else{
     }//else if(state==METEN_HOOGTE){
-        
-    else if(state==METEN_RICHTING){
+
+    else if(state==METEN_RICHTING) {
         lcd.cls();
-        if(current_herhalingen<=200){
+        if(current_herhalingen<=200) {
             lcd.locate(0,0);
             lcd.printf("Richting bepalen:");
             lcd.locate(0,1);
             lcd.printf("span aan per vak");
         }//if(current_herhalingen<=200)
-        else if(200<=current_herhalingen && current_herhalingen<1200){
+        else if(200<=current_herhalingen && current_herhalingen<1200) {
             lcd.locate(0,0);
             lcd.printf("Vak %d",doel);
-            if(current_herhalingen<=400){
-                lcd.locate(0,1); 
+            if(current_herhalingen<=400) {
+                lcd.locate(0,1);
                 lcd.printf("nog 5 sec.");
             }//if(current_herhalingen<=400)
-            else if(current_herhalingen<=600){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<=600) {
+                lcd.locate(0,1);
                 lcd.printf("nog 4 sec.");
             }//else if(current_herhalingen<=600)
-            else if(current_herhalingen<=800){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<=800) {
+                lcd.locate(0,1);
                 lcd.printf("nog 3 sec.");
             }//else if(current_herhalingen<=800)
-            else if(current_herhalingen<=1000){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<=1000) {
+                lcd.locate(0,1);
                 lcd.printf("nog 2 sec.");
             }//else if(current_herhalingen<=1000)
-            else if(current_herhalingen<1200){
-                lcd.locate(0,1); 
+            else if(current_herhalingen<1200) {
+                lcd.locate(0,1);
                 lcd.printf("nog 1 sec.");
             }//else if(current_herhalingen<=1200)
         }//else if(200<=current_herhalingen<=1200)
-        else if(current_herhalingen<=2200){
+        else if(current_herhalingen<=2200) {
             lcd.locate(0,0);
             lcd.printf("Vak %d akkoord?",doel_richting);
             lcd.locate(0,1);
             lcd.printf("Span aan");
         }//else if(current_herhalingen<=1600)
-        else{
+        else {
             lcd.locate(0,0);
             lcd.printf("Opnieuw richting");
             lcd.locate(0,1);
             lcd.printf("bepalen");
         }//else
     }//else if(state==METEN_RICHTING){
-        
-    else if(state==INSTELLEN_RICHTING){
+
+    else if(state==INSTELLEN_RICHTING) {
+        pc.printf("i");
         lcd.cls();
         lcd.locate(0,0);
         lcd.printf("Instellen hoek");
         lcd.locate(0,1);
-        lcd.printf("Even geduld...");
+        lcd.printf("Even ");
+        pc.printf("e");
     }//else if(state==INSTELLEN_RICHTING){
-        
-    else if(state==SLAAN){
+
+    else if(state==SLAAN) {
         lcd.cls();
         lcd.locate(0,0);
         lcd.printf("Slaan, pas op");
         lcd.locate(0,1);
         lcd.printf("Let's pray");
     }//else if(state==INSTELLEN_RICHTING){
-        
-    else if(state==RETURN2RUST){
+
+    else if(state==RETURN2RUST) {
         lcd.cls();
         lcd.locate(0,0);
         lcd.printf("Terug naar");
         lcd.locate(0,1);
         lcd.printf("0-positie");
     }//else if(state==INSTELLEN_RICHTING){
-        
-    else{     
+
+    else {
         lcd.cls();
         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
     }//else{
 }
 
-int main(){
+int main()
+{
     pc.baud(115200);
     arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
     arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);