Revision 28:97b9e50c1180, committed 2016-10-28

Comitter:

MBroek

Date:

Fri Oct 28 07:56:12 2016 +0000

Parent:

25:553b0ca967fc

Child:

29:2b711fc9a5b2

Commit message:

Initialisatie motor 2 aangepast. Filter toegevoegd in PID

Changed in this revision

--- a/main.cpp	Wed Oct 26 08:00:58 2016 +0000
+++ b/main.cpp	Fri Oct 28 07:56:12 2016 +0000
@@ -18,8 +18,8 @@
 MODSERIAL pc(USBTX, USBRX);
 #define SERIALBAUD 115200
 
-QEI encoder_motor1(D10,D11,NC,64);
-QEI encoder_motor2(D12,D13,NC,64);
+QEI encoder_motor1(D10,D11,NC,64, QEI::X4_ENCODING);
+QEI encoder_motor2(D12,D13,NC,64, QEI::X4_ENCODING);
 
 
 // Definieren van de Motorpins ---------------------------------------
@@ -36,8 +36,8 @@
 DigitalIn EMG_sim1(SW2);
 DigitalIn EMG_sim2(SW3);
 
-AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
-AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
+//AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
+//AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
 
 
 DigitalOut ledred(LED_RED, 1);
@@ -47,6 +47,8 @@
 AnalogIn    emg0( A0 );         //  right biceps
 AnalogIn    emg1( A1 );         //  left biceps
 
+AnalogIn init_pot(A3);          // POT meter 
+
 
 //Definieren van de tickers ------------------------------------------
 Ticker motor_ticker;                        // Deze ticker activeert het motor_runs programma, dit leest motor pos en ref pos uit, rekent de PID uit en stuurt met dit de motoren
@@ -72,11 +74,18 @@
 BiQuad bq9 = bq7;
 BiQuad bq10 = bq9;
 
+BiQuadChain bqc5;       //  Low-pass filter (10Hz) voor PID
+BiQuad bq11( 1.32937e-05, 2.65875e-05, 1.32937e-05, -1.77831e+00, 7.92447e-01 );
+BiQuad bq12( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.89342e+00, 9.08464e-01 );
+
+
+
+
 
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
 
-enum states_enum{off, init, run};                   // De states waar de main loop doorheen loopt, off is uit, init is intialiseren en run is het aanturen vd motoren met emg.
+enum states_enum{off, init, init_fish, run};                   // De states waar de main loop doorheen loopt, off is uit, init is intialiseren en run is het aanturen vd motoren met emg.
 states_enum states = off;
 
 double ref_pos_prev_m1 = 0.0;                          // De initiele ref_pos_pref
@@ -100,19 +109,21 @@
 
 volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
 
-const double Kp_1 = 0.3;                    // De PID variablen voor motor 1
-const double Kd_1 = 0.0;
-const double Ki_1 = 0.0;
+const double Kp_1 = 0.07;                    // De PID variablen voor motor 1
+const double Kd_1 = 0.15;
+const double Ki_1 = 0.1;
 
 const double Kp_2 = 1.0;                    // De PID variablen voor motor 2
-const double Kd_2 = 0.0;
-const double Ki_2 = 0.0;
+const double Kd_2 = 0.4;
+const double Ki_2 = 0.1;
 
-const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
-const double vrijheid_m2_meter = 0.5;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
+const double vrijheid_m1_rad = 0.4*pi;      // Dit is de uiterste postitie vd arm in radialen
+const double vrijheid_m2_meter = 0.2;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
 
 const double initial_pos_m1 = vrijheid_m1_rad;                // Startin posities van de motoren
-const double initial_pos_m2 = 0; 
+const double initial_pos_m2 = vrijheid_m2_meter; 
+
+double position_motor1;
 
 double position_motor2; 
 double position_motor2_prev = initial_pos_m2;    
@@ -141,8 +152,6 @@
 double max_out0 = 0;            //  Set initial maximum values of the signals to 0, the code will check if there's a new maximum value every iteration
 double max_out1 = 0;
 
-int final_signal;               // Dit is waarmee de reference position gezocht gaat worden.
-
 
 
 
@@ -157,6 +166,9 @@
         states = init;
     }
     else if(states == init){
+        states = init_fish;
+    }
+    else if(states == init_fish){
         states = run;
     }
     else{
@@ -225,6 +237,7 @@
     else
     {   digital1 = 0;   }
     
+    /*
     scope.set(0,in0);           //  EMG signal 0    (right biceps)
     scope.set(1,in1);           //  EMG signal 1    (left biceps)
     scope.set(2,digital0);      //  Signal 0 after filtering, detrending, rectification, digitalization
@@ -232,6 +245,7 @@
     scope.set(4,digital0 - digital1);         //  Final output signal
     
     scope.send();               //  Send all channels to the PC at once
+    */
 
     return digital0 - digital1;         //  Subtract the digital signals (right arm gives positive values, left arm give negative values)
 }
@@ -241,7 +255,8 @@
 // Functie voor het vinden van de reference position van motor 1 --------------------
 double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
     double ref_pos;
-    final_signal = get_EMG();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
+    // int final_signal = get_EMG();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
+    int final_signal = EMG_sim1.read() - EMG_sim2.read();
     switch(final_signal){
         case 0 :
             ref_pos = ref_pos_prev;
@@ -303,7 +318,7 @@
 double PID_controller(double ref_pos, double mot_pos, double &e_prev, double &e_int, const double Kp, const double Kd, const double Ki){
     double error = ref_pos - mot_pos;       // error uitrekenen
     double error_dif = (error-e_prev)/T_motor_function;      // De error differentieren
-    //error_dif = .....       // Filter biquad poep
+    error_dif = bqc5.step(error_dif);       // Filter biquad poep
     e_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
     e_int = e_int + T_motor_function*error;             // De error integreren
     return Kp*error + Ki*e_int + Kd*error_dif;       // De uiteindelijke PID output
@@ -354,11 +369,27 @@
 }
 
 
-// De initialiseren functie ------------------------------------------------------------
-void initialise(){       // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
+// De initialisatie functie --------------------------------------------------------------------
+void initialise(){
+    ledred.write(1);
+    ledblue.write(0);
+    if (flag1){
+        double init_m2_speed = init_pot;
+            set_motor2(init_m2_speed);
+        }
+}
+
+
+// De initialiseren beeindigen functie ------------------------------------------------------------
+void initialise_finish(){       // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
+    ledblue.write(1);
     ref_pos_prev_m1 = initial_pos_m1;
     ref_pos_prev_m2 = initial_pos_m2;
+    position_motor1 = initial_pos_m1;
+    position_motor2 = initial_pos_m2;
+    position_motor2_prev = initial_pos_m2;
     ledgreen.write(0);
+    ledred.write(0);
     encoder_motor1.reset();
     encoder_motor2.reset();             
     motor1_speed_pin = 0;
@@ -373,10 +404,11 @@
             switch (motor_that_runs){
                 case motor1 :      // In deze case draait alleen motor 1
                     reference_pos_m1 = get_reference_position_m1(ref_pos_prev_m1, vrijheid_m1_rad);
-                    PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
-                    PID_output_2 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1 
+                    position_motor1 = get_position_m1(rad_per_count);
+                    PID_output_1 = PID_controller(reference_pos_m1, position_motor1, error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
+                    //PID_output_2 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1 
                     set_motor1(PID_output_1);
-                    set_motor2(PID_output_2);
+                    //set_motor2(-PID_output_1);
                     encoder_motor2.reset();
                     break;
                 case motor2 :
@@ -409,6 +441,15 @@
 {
     pc.baud(SERIALBAUD);
     
+    bqc1.add( &bq1 ).add( &bq2 ).add( &bq3 );
+    bqc2.add( &bq4 ).add( &bq5 ).add( &bq6 );
+    bqc3.add( &bq7 ).add( &bq8 );
+    bqc4.add( &bq9 ).add( &bq10 );
+    bqc5.add( &bq11 ).add( &bq12 );
+    
+    
+    
+    
     state_switch_button.fall(&state_changer);        // Switcht states
     motor_switch_button.fall(&motor_switch);        // Switcht motors
     
@@ -425,12 +466,15 @@
             case init :          // Hier voert hij alleen het initialiseren van motor 1 uit
                 initialise();      
                 break;
+            case init_fish :
+                initialise_finish();
+                break;
             case run :
                 running_motors();       // Hier voert hij het programma voor het aansturen vd motors uit
                 break;
             
         }
-        //call_HIDscope(final_signal, reference_pos_m1, get_position_m1(rad_per_count));
+        call_HIDscope(PID_output_1, reference_pos_m1, position_motor1);
     }  
 }
         

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Fri Oct 28 07:56:12 2016 +0000
@@ -0,0 +1,422 @@
+
+
+// HET DEFINIEREN VAN ALLES ==========================================================================================
+
+// Includen van alle libraries ---------------------------------------
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "math.h"
+#include "HIDScope.h"
+
+// Definieren van de HIDscope ----------------------------------------
+HIDScope scope(3);               
+
+
+// Definieren van de Serial en Encoder -------------------------------
+MODSERIAL pc(USBTX, USBRX);
+#define SERIALBAUD 115200
+
+QEI encoder_motor1(D10,D11,NC,8400, QEI::X4_ENCODING);
+QEI encoder_motor2(D12,D13,NC,8400, QEI::X4_ENCODING);
+
+
+// Definieren van de Motorpins ---------------------------------------
+DigitalOut motor1_direction_pin(D7);
+PwmOut motor1_speed_pin(D6);
+
+DigitalOut motor2_direction_pin(D4);
+PwmOut motor2_speed_pin(D5);
+
+
+//Definieren van bord-elementen --------------------------------------
+InterruptIn motor_switch_button(D3);
+InterruptIn state_switch_button(D2);
+DigitalIn EMG_sim1(SW2);
+DigitalIn EMG_sim2(SW3);
+
+AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
+AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
+
+
+DigitalOut ledred(LED_RED, 1);
+DigitalOut ledgreen(LED_GREEN, 1);
+DigitalOut ledblue(LED_BLUE, 1);
+
+
+//Definieren van de tickers ------------------------------------------
+Ticker motor_ticker;                        // Deze ticker activeert het motor_runs programma, dit leest motor pos en ref pos uit, rekent de PID uit en stuurt met dit de motoren
+Ticker hidscope_ticker;
+
+
+
+// HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
+
+enum states_enum{off, init, run};                   // De states waar de main loop doorheen loopt, off is uit, init is intialiseren en run is het aanturen vd motoren met emg.
+states_enum states = off;
+
+double ref_pos_prev_m1 = 0.0;                          // De initiele ref_pos_pref
+double ref_pos_prev_m2 = 0.0;
+
+int counts1;                                // Pulses van motoren
+int counts2;
+
+const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
+const double rad_per_count = (2.0*pi)/8400.0;       // Aantal rad per puls uit encoder
+
+const double radius_tandwiel = 0.008;
+const double meter_per_count = rad_per_count * radius_tandwiel;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
+
+const double v_max_karretje = 8.4*radius_tandwiel;                  // Maximale snelheid karretje, gelimiteerd door de v_max vd motor
+
+double error1_int = 0.00000;                 // Initiele error integral waardes
+double error2_int = 0.00000;
+
+const double T_motor_function = 0.1;           // Periode van de frequentie van het aanroepen van onder andere positiechecker (get_position) en de rest vd motor functie
+
+volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
+
+const double Kp_1 = 0.5;                    // De PID variablen voor motor 1
+const double Kd_1 = 0.1;
+const double Ki_1 = 0.1;
+
+const double Kp_2 = 1.0;                    // De PID variablen voor motor 2
+const double Kd_2 = 0.0;
+const double Ki_2 = 0.0;
+
+const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
+const double vrijheid_m2_meter = 0.5;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
+
+const double initial_pos_m1 = vrijheid_m1_rad;                // Startin posities van de motoren
+const double initial_pos_m2 = 0; 
+
+double position_motor2; 
+double position_motor1;
+double position_motor2_prev = initial_pos_m2;    
+
+double PID_output_1 = 0.0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
+double PID_output_2 = 0.0;
+double PID_output_2_1 = 0.0;
+
+double error_prev_1 = 0.0;                      // De initiele previous error
+double error_int_1 = 0.0;                       // De initiele intergral error
+
+double error_prev_2 = 0.0;
+double error_int_2 = 0.0;
+
+double reference_pos_m2;                      // De reference positie waar de motor heen wil afhankelijk van het EMG signaal
+double reference_pos_m1;
+
+enum which_motor{motor1, motor2};               // enum van de motoren
+which_motor motor_that_runs = motor1;
+
+
+
+
+
+
+// ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
+
+
+// De statechanger -----------------------------------------------------------
+void state_changer(){
+    if(states == off){
+        states = init;
+    }
+    else if(states == init){
+        states = run;
+    }
+    else{
+        states = off;
+    }
+}
+
+
+// Functie voor het switchen tussen de motors ------------------------------
+void motor_switch(){
+    if(motor_that_runs == motor1){
+        motor_that_runs = motor2;
+        }
+            else{
+                motor_that_runs = motor1;
+            }
+}
+
+
+
+// Functie voor het vinden van de positie van motor 1 ---------------------
+double get_position_m1(const double radpercount){          //returned de positie van de motor in radialen (positie vd arm in radialen)
+    counts1 = encoder_motor1.getPulses();           // Leest aantal pulses uit encoder af
+    return radpercount * counts1 + initial_pos_m1;      // rekent positie motor1 uit en geeft deze als output vd functie
+}
+
+
+// Functie voor vinden van de positie van motor 2 -----------------------------
+double get_position_m2(const double meterpercount, double mot_pos_prev){     // returned de positie van het karretje in meter
+    double mot_pos;
+    counts2 = encoder_motor2.getPulses();               // leest aantal pulses vd encoder af
+    mot_pos = meterpercount * counts2 + mot_pos_prev;
+    encoder_motor2.reset();
+    mot_pos_prev = mot_pos;
+    return mot_pos_prev;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
+}
+
+
+// Functie voor het vinden van de reference position van motor 1 --------------------
+double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
+    double ref_pos;
+    int final_signal = EMG_sim1.read() - EMG_sim2.read();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
+    switch(final_signal){
+        case 0 :
+            ref_pos = ref_pos_prev;
+            break;
+        case 1 :
+            ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad;         // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
+            break;
+        case -1 :
+            ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad;
+            break;
+    }
+    if(ref_pos >= vrijheid_rad){
+        ref_pos = vrijheid_rad;
+    }
+    if(ref_pos <= -vrijheid_rad){
+        ref_pos = -vrijheid_rad;
+    }
+    ref_pos_prev = ref_pos;
+    return ref_pos_prev;            // vrijheid_rad is de uiterste positie vd arm in radialen                                                                    
+}
+
+
+// Functie voor het vinden van de reference position van motor 2 --------------------
+double get_reference_position_m2(double &ref_pos_prev, const double vrijheid_meter){
+    double ref_pos;
+    int final_signal = EMG_sim1.read() - EMG_sim2.read();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
+    switch(final_signal){
+        case 0 :
+            ref_pos = ref_pos_prev;
+            break;
+        case 1 :
+            ref_pos = ref_pos_prev + T_motor_function*v_max_karretje;           // Hierdoor veranderd de ref_pos met de maximale snelheid vd motor (karretje)
+            break;
+        case -1 :
+            ref_pos = ref_pos_prev - T_motor_function*v_max_karretje;
+            break;
+    }
+    if(ref_pos >= vrijheid_meter){
+        ref_pos = vrijheid_meter;
+    }
+    if(ref_pos <= 0){
+        ref_pos = 0;
+    }
+    ref_pos_prev = ref_pos;
+    return ref_pos_prev;                                                                                
+}
+
+
+// HIDScope functie ----------------------------------------------------------------------
+void set_scope(double input1, double input2, double input3){
+    scope.set(0, input1); 
+    scope.set(1, input2);
+    scope.set(2, input3);
+    scope.send();
+}
+
+
+// De PID-controller voor de motors -------------------------------------------------
+double PID_controller(double ref_pos, double mot_pos, double &e_prev, double &e_int, const double Kp, const double Kd, const double Ki){
+    double error = ref_pos - mot_pos;       // error uitrekenen
+    double error_dif = (error-e_prev)/T_motor_function;      // De error differentieren
+    //error_dif = .....       // Filter biquad poep
+    e_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
+    e_int = e_int + T_motor_function*error;             // De error integreren
+    return Kp*error + Ki*e_int + Kd*error_dif;       // De uiteindelijke PID output
+}
+       
+
+// Motor 1 besturen ---------------------------------------------------------------------
+void set_motor1(double motor_input){        // functie die de motor aanstuurt mt als input de PID-output
+    if (motor_input >= 0){                  // Dit checkt welke kant de motor op moet draaien
+        motor1_direction_pin = 1;           // Clockwise
+    }
+        else {
+            motor1_direction_pin = 0;       // CounterClockwise
+        }
+    if (fabs(motor_input)>1){               // Dit fixed de PwmOutput op maximaal 1
+        motor_input = 1;
+    }
+    motor1_speed_pin = fabs(motor_input);   // Dit geeft de uiteindelijke input door aan de motor
+}
+ 
+ 
+// Motor 2 besturen ---------------------------------------------------------------------
+void set_motor2(double motor_input){        // functie die de motor aanstuurt mt als input de PID-output
+    if (motor_input >= 0){                  // Dit checkt welke kant de motor op moet draaien
+        motor2_direction_pin = 1;           // Clockwise
+    }
+        else {
+            motor2_direction_pin = 0;       // CounterClockwise
+        }
+    if (fabs(motor_input)>1){               // Dit fixed de PwmOutput op maximaal 1
+        motor_input = 1;
+    }
+    motor2_speed_pin = fabs(motor_input);   // Dit geeft de uiteindelijke input door aan de motor
+}
+
+
+// De go-flag functies -----------------------------------------------------------------
+void flag1_activate(){flag1=true;}           // Activeert de flag
+void flag2_activate(){flag2=true;}   
+
+
+// Dit doet de robot als hij niets doet ------------------------------------------------------
+void robot_is_off(){
+    ledgreen.write(1);              
+    ledred.write(0);               // Indicator ik ben uit
+    motor1_speed_pin = 0;           // Uitzetten vd motoren
+    motor2_speed_pin = 0;
+}
+
+
+// De initialiseren functie ------------------------------------------------------------
+void initialise(){       // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
+    ref_pos_prev_m1 = initial_pos_m1;
+    ref_pos_prev_m2 = initial_pos_m2;
+    ledgreen.write(0);
+    encoder_motor1.reset();
+    encoder_motor2.reset();             
+    motor1_speed_pin = 0;
+}  
+
+
+// De functie die de motoren aanstuurt -------------------------------------------------
+void running_motors(){              // Deze functie kijkt welke motor moet draaien en rekent dan de PID uit en geeft dit door aan de motor input functie
+    if (flag1){
+            flag1 = false;
+            ledred.write(1);
+            switch (motor_that_runs){
+                case motor1 :      // In deze case draait alleen motor 1
+                    reference_pos_m1 = get_reference_position_m1(ref_pos_prev_m1, vrijheid_m1_rad);
+                    position_motor1 = get_position_m1(rad_per_count);
+                    PID_output_1 = PID_controller(reference_pos_m1, position_motor1, error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
+                    //PID_output_2_1 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1 
+                    set_motor1(PID_output_1);
+                    set_motor2(PID_output_1);
+                    encoder_motor2.reset();
+                    break;
+                case motor2 :
+                    reference_pos_m2 = get_reference_position_m2(ref_pos_prev_m2, vrijheid_m2_meter);
+                    position_motor2 = get_position_m2(meter_per_count, position_motor2_prev);
+                    PID_output_2 = PID_controller(reference_pos_m2, position_motor2, error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
+                    PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);   // Zorgt ervoor dat motor1 op de dezelfde positie blijft als motor 2 draait.
+                    set_motor2(PID_output_2);
+                    set_motor1(PID_output_1);
+                    position_motor2_prev = position_motor2;
+                    break;
+            }
+    }
+}
+
+
+// De HIDscope debug functie ----------------------------------------------------------------
+void call_HIDscope(double input_1, double input_2, double input_3){             // Deze functie roept de HIDscope aan
+        if (flag2){
+            flag2 = false;
+            set_scope(input_1, input_2, input_3);
+        }
+}
+
+
+
+
+// DE MAIN =================================================================================================================
+int main()
+{
+    pc.baud(SERIALBAUD);
+    
+    state_switch_button.fall(&state_changer);        // Switcht states
+    motor_switch_button.fall(&motor_switch);        // Switcht motors
+    
+
+    
+    motor_ticker.attach(&flag1_activate, T_motor_function);                  // Activeert de go-flag van motor positie 
+    hidscope_ticker.attach(&flag2_activate,0.01);   // Leest ref pos en mot pos uit, rekent PID uit en stuurt motoren aan.
+    
+    while(1){
+        switch(states){
+            case off :                    // Dan staat het programma stil en gebeurt er niks
+                robot_is_off();
+                break;
+            case init :          // Hier voert hij alleen het initialiseren van motor 1 uit
+                initialise();      
+                break;
+            case run :
+                running_motors();       // Hier voert hij het programma voor het aansturen vd motors uit
+                break;
+            
+        }
+        call_HIDscope(PID_output_1, reference_pos_m1, position_motor1);
+    }  
+}
+        
+        
+        
+        
+        
+        
+        
+        
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+