Revision 15:3f3d87513a6b, committed 2016-10-23

Comitter:

MBroek

Date:

Sun Oct 23 09:42:26 2016 +0000

Parent:

14:f51d51395803

Child:

16:5a749b319276

Commit message:

opgeruimde main-loop, werkende initialisatie voor motor1. Is weer stabiel. Uiteindelijk PID coefficienten checken met robot.

Changed in this revision

--- a/main.cpp	Fri Oct 21 09:12:36 2016 +0000
+++ b/main.cpp	Sun Oct 23 09:42:26 2016 +0000
@@ -50,7 +50,7 @@
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
 
-enum states_enum{off, init_m1, 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_m1, init_m2, finish_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;
 
 bool starting = false;                               // conditie om programma te starten
@@ -69,7 +69,7 @@
 int counts2;
 
 const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
-const double rad_per_count = pi/8400.0;       // Aantal rad per puls uit encoder
+const double rad_per_count = 2.0*pi/8400.0;       // Aantal rad per puls uit encoder
 
 const double radius_tandwiel = 1.0;
 const double meter_per_count = rad_per_count * radius_tandwiel;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
@@ -81,9 +81,9 @@
 
 volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
 
-const double Kp_1 = 1.0000000;                    // De PID variablen voor motor 1
-const double Kd_1 = 0.1;
-const double Ki_1 = 0.3;
+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_2 = 1.0000000;                    // De PID variablen voor motor 2
 const double Kd_2 = 0.1;
@@ -107,7 +107,10 @@
 double error_prev_2 = 0.0;
 double error_int_2 = 0.0;
 
-bool which_motor = false;
+//bool which_motor = false;
+enum which_motor{motor1, motor2};
+which_motor motor_that_runs = motor1;
+
 
 
 
@@ -117,7 +120,7 @@
 
 // De program starter ------------------------------------------------
 void start_program(){
-    starting = true;
+    states = init_m1;
     pc.printf("Starting\n\r");
 }
 
@@ -131,14 +134,30 @@
 
 // De initialiseer beeindiger knop ------------------------------------------------
 void stop_initializing(){
-    initializing = false;
+    states = run;
+    ledred.write(1);
+    encoder_motor1.reset();
     pc.printf("Initializing ended\r\n");
 }
 
+/*
+// De statechanger -----------------------------------------------------------
+void state_changer(){
+    if(states == off){
+        states = init_m1;
+    }
+    ()
+*/
 
 // Functie voor het switchen tussen de motors ------------------------------
 void motor_switch(){
-    which_motor = !which_motor;     // =0 activeert motor1 en =1 activeert motor2
+    if(motor_that_runs == motor1){
+        motor_that_runs = motor2;
+        }
+            else{
+                motor_that_runs = motor1;
+            }
+
 }
 
 
@@ -236,6 +255,35 @@
 }
 
 
+// De functie die de motoren aanstuurt -------------------------------------------------
+void running_motors(){
+    if (flag1){
+            flag1 = false;
+            ledgreen = !ledgreen;
+            switch (motor_that_runs){
+                case motor1 :      // In deze case draait alleen motor 1
+                    PID_output_1 = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
+                    set_motor1(PID_output_1);
+                    break;
+                case motor2 :
+                    PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter), get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
+                    set_motor2(PID_output_2);
+                    break;
+            }
+    }
+}
+
+
+// De HIDscope debug functie ----------------------------------------------------------------
+void call_HIDscope(double input_1, double input_2, double input_3){
+        if (flag2){
+            flag2 = false;
+            set_scope(input_1, input_2, input_3);
+        }
+}
+
+
+
 
 // DE MAIN =================================================================================================================
 int main()
@@ -256,17 +304,18 @@
             case off :              // Dan staat het programma stil en gebeurt er niks
                 break;
             case init_m1 :          // Hier voert hij alleen het initialiseren van motor 1 uit
-                initialise_m1;      
+                initialise_m1();      
                 break;
-            case 
-            
+            case run :
+                running_motors();
+                break;
             
-        
-        
-    }
-        
-        
-        
+        }
+        call_HIDscope(PID_output_1_in, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(meter_per_count));
+    }  
+    motor1_speed_pin = 0;   //Dit zet de motoren uit nadat de kill switch is gebruikt
+    motor2_speed_pin = 0;
+}
         
         
         
@@ -275,44 +324,6 @@
         
         
         
-    if(starting){                                   // Activeert het programma
-    
-    // Initialisatie ---------------------------------------
-    while(initializing){
-      if (flag1){
-          flag1 = false;
-          ledred = !ledred;
-          PID_output_1_in = PID_controller(reference_position_motor1, get_position_m1(rad_per_count)+starting_pos_m1, error_prev_1_in, error_int_1_in, Kp_1, Kd_1, Ki_1);
-          set_motor1(PID_output_1_in);
-      }
-    }
-      
-ledred.write(1);
-encoder_motor1.reset();
-    
-    while(safe){            // Draait loop zolang alles veilig is.
-        if (flag1){
-            flag1 = false;
-            ledgreen = !ledgreen;
-            if (!which_motor){      // als which_motor=0 gaat motor 1 lopen
-                PID_output_1 = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
-                set_motor1(PID_output_1);
-            }
-                else{
-                    PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter), get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
-                    set_motor2(PID_output_2);
-                }
-        }
-        if (flag2){
-            flag2 = false;
-            set_scope(PID_output_1, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(meter_per_count));
-        }
-    }
-    
-    motor1_speed_pin = 0;   //Dit zet de motoren uit nadat de kill switch is gebruikt
-    motor2_speed_pin = 0;
-}
-}