Diff: main.cpp

Revision:

21:3db3f2d56d56

Parent:

20:2fdb069ffcae

Child:

22:6dfe5554b96e

--- a/main.cpp	Mon Oct 24 14:15:21 2016 +0000
+++ b/main.cpp	Mon Oct 24 20:35:10 2016 +0000
@@ -52,7 +52,7 @@
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
 
-enum states_enum{off, init_m1, init_m2, finish_init_m1, finish_init_m2, 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, 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 error_prev_1_in = 0.0, error_int_1_in = 0.0; // De error waardes voor het initialiseren
@@ -61,6 +61,9 @@
 double error_prev_2_in = 0.0, error_int_2_in = 0.0; // De error waardes voor het initialiseren
 double PID_output_2_in = 0.0;                         // De PID output voor het initialiseren
 
+double ref_pos_prev_m1 = 0.0;                          // De initiele ref_pos_pref
+double ref_pos_prev_m2 = 0.0;
+
 double position_motor1;
 
 int counts1;                                // Pulses van motoren
@@ -75,13 +78,13 @@
 double error1_int = 0.00000;                 // Initiele error integral waardes
 double error2_int = 0.00000;
 
-const double T_getpos = 0.01;           // Periode van de frequentie van het aanroepen van de positiechecker (get_position)
+const double T_motor_function = 0.01;           // 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.1;                    // De PID variablen voor motor 1
-const double Kd_1 = 0.01;
-const double Ki_1 = 0.01;
+const double Kp_1 = 0.5;                    // De PID variablen voor motor 1
+const double Kd_1 = 0.0;
+const double Ki_1 = 0.0;
 
 const double Kp_2 = 1.0000000;                    // De PID variablen voor motor 2
 const double Kd_2 = 0.1;
@@ -90,11 +93,8 @@
 const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
 const double vrijheid_m2_meter = 0.25;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
 
-const double initial_pos_m1 = 0.5*pi;                // Startin posities van de motoren
-const double initial_pos_m2 = 0.25;
-
-double starting_position_motor1 = 0;     // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
-double starting_position_motor2 = 0;           
+const double initial_pos_m1 = vrijheid_m1_rad;                // Startin posities van de motoren
+const double initial_pos_m2 = vrijheid_m2_meter;       
 
 double PID_output_1 = 0.0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
 double PID_output_2 = 0.0;
@@ -122,18 +122,9 @@
 // De statechanger -----------------------------------------------------------
 void state_changer(){
     if(states == off){
-        states = init_m1;
-    }
-    else if(states == init_m1){
-        states = finish_init_m1;
+        states = init;
     }
-    else if(states == finish_init_m1){
-        states = init_m2;
-    }
-    else if(states == init_m2){
-        states = finish_init_m2;
-    }
-    else if(states == finish_init_m2){
+    else if(states == init){
         states = run;
     }
     else{
@@ -157,7 +148,7 @@
 // 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;      // rekent positie motor1 uit en geeft deze als output vd functie
+    return radpercount * counts1 + initial_pos_m1;      // rekent positie motor1 uit en geeft deze als output vd functie
 }
 
 
@@ -169,16 +160,35 @@
 
 
 // Functie voor het vinden van de reference position van motor 1 --------------------
-double get_reference_position_m1(const double aantal_rad){
-    double ref_pos = EMG_sim1.read() - EMG_sim2.read();     // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
-    return ref_pos * aantal_rad;            // Aantal rad is de uiterste positie vd arm in radialen                                                                    
+double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
+    double ref_pos_m1;
+    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_m1 = ref_pos_prev;
+            break;
+        case 1 :
+            ref_pos_m1 = ref_pos_prev + T_motor_function*vrijheid_rad;
+            break;
+        case -1 :
+            ref_pos_m1 = ref_pos_prev - T_motor_function*vrijheid_rad;
+            break;
+    }
+    if(ref_pos_m1 >= vrijheid_rad){
+        ref_pos_m1 = vrijheid_rad;
+    }
+    if(ref_pos_m1 <= -vrijheid_rad){
+        ref_pos_m1 = -vrijheid_rad;
+    }
+    ref_pos_prev = ref_pos_m1;
+    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(const double aantal_meter){
-    double ref_pos = EMG_sim1.read() - EMG_sim2.read();     // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
-    return ref_pos * aantal_meter;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
+    double final_signal = EMG_sim1.read() - EMG_sim2.read();     // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
+    return final_signal * aantal_meter;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
 }
    
 
@@ -194,10 +204,10 @@
 // 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_getpos;      // De error differentieren
+    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_getpos*error;             // De error integreren
+    e_int = e_int + T_motor_function*error;             // De error integreren
     return Kp*error + Ki*e_int + Kd*error_dif;       // De uiteindelijke PID output
 }
        
@@ -237,47 +247,24 @@
 void flag2_activate(){flag2=true;}   
 
 
-// De initialiseren functie van motor 1 ------------------------------------------------------------
-void initialise_m1(){       // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
-    if (flag1){
-        flag1 = false;
-        ledred = !ledred;
-        ledblue.write(1);
-        PID_output_1_in = PID_controller(starting_position_motor1, get_position_m1(rad_per_count)+initial_pos_m1, error_prev_1_in, error_int_1_in, Kp_1, Kd_1, Ki_1);     // PID met een vaste ref pos.
-        set_motor1(PID_output_1_in);
-    }
-}
-
-
-// De initialiseren functie van motor 2 ------------------------------------------------------------
-void initialise_m2(){           // Hetzelfde als hierboven
-    if (flag1){
-        flag1 = false;
-        ledblue = !ledblue;
-        ledgreen.write(1);
-        PID_output_2_in = PID_controller(starting_position_motor2, get_position_m2(meter_per_count)+initial_pos_m2, error_prev_2_in, error_int_2_in, Kp_2, Kd_2, Ki_2);       // PID met vaste ref pos.
-        set_motor2(PID_output_2_in);
-    }
-}
-
-
-// De functies voor het afronden van de initialisatie ----------------------------------------
-void finish_initialising_m2(){              // Deze functie rond het initialiseren af, hij reset de encoder en zet de motor uit
-    encoder_motor2.reset();
-    ledblue.write(1);
-    ledred.write(0);
+// 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 functies voor het afronden van de initialisatie ----------------------------------------
-void finish_initialising_m1(){          // Zelfde als hierboven
-    encoder_motor1.reset();             
-    ledred.write(1);
+// 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 -------------------------------------------------
@@ -285,10 +272,9 @@
     if (flag1){
             flag1 = false;
             ledred.write(1);
-            ledgreen = !ledgreen;
             switch (motor_that_runs){
                 case motor1 :      // In deze case draait alleen motor 1
-                    reference_pos_m1 = get_reference_position_m1(vrijheid_m1_rad);
+                    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 
                     set_motor1(PID_output_1);
@@ -306,15 +292,6 @@
 }
 
 
-// Dit doet de robot als hij niets doet ------------------------------------------------------
-void robot_is_off(){
-    ledgreen.write(1);              
-    ledblue.write(0);               // Indicator ik ben uit
-    motor1_speed_pin = 0;           // Uitzetten vd motoren
-    motor2_speed_pin = 0;
-}
-
-
 // De HIDscope debug functie ----------------------------------------------------------------
 void call_HIDscope(double input_1, double input_2, double input_3){             // Deze functie roept de HIDscope aan
         if (flag2){
@@ -336,7 +313,7 @@
     
 
     
-    motor_ticker.attach(&flag1_activate,0.1);                  // Activeert de go-flag van motor positie 
+    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){
@@ -344,24 +321,15 @@
             case off :                    // Dan staat het programma stil en gebeurt er niks
                 robot_is_off();
                 break;
-            case init_m1 :          // Hier voert hij alleen het initialiseren van motor 1 uit
-                initialise_m1();      
-                break;
-            case finish_init_m1 :       // Hier beeindigt hij het initialiseren
-                finish_initialising_m1();
+            case init :          // Hier voert hij alleen het initialiseren van motor 1 uit
+                initialise();      
                 break;
-            case init_m2 :
-                initialise_m2();        // Voert de initialsatie van motor 2 uit
-                break;
-            case finish_init_m2 :
-                finish_initialising_m2();       // beeindigt de initialisatie van motor 2
-                break;  
             case run :
                 running_motors();       // Hier voert hij het programma voor het aansturen vd motors uit
                 break;
             
         }
-        call_HIDscope(PID_output_1_in, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(meter_per_count));
+        call_HIDscope(PID_output_1, reference_pos_m1, get_position_m1(meter_per_count));
     }  
 }