Diff: main.cpp

Revision:

16:5a749b319276

Parent:

15:3f3d87513a6b

Child:

17:525b785f007a

--- a/main.cpp	Sun Oct 23 09:42:26 2016 +0000
+++ b/main.cpp	Sun Oct 23 10:56:14 2016 +0000
@@ -30,8 +30,8 @@
 
 
 //Definieren van bord-elementen --------------------------------------
-InterruptIn kill_switch(D8);
-InterruptIn test_button(D9);
+InterruptIn motor_switch_button(D8);
+InterruptIn state_switch_button(D9);
 AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
 AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
 
@@ -40,6 +40,7 @@
 
 DigitalOut ledred(LED_RED, 1);
 DigitalOut ledgreen(LED_GREEN, 1);
+DigitalOut ledblue(LED_BLUE, 1);
 
 
 //Definieren van de tickers ------------------------------------------
@@ -50,7 +51,7 @@
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
 
-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.
+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.
 states_enum states = off;
 
 bool starting = false;                               // conditie om programma te starten
@@ -58,6 +59,10 @@
 
 double error_prev_1_in = 0.0, error_int_1_in = 0.0; // De error waardes voor het initialiseren
 double PID_output_1_in = 0.0;                         // De PID output voor het initialiseren
+
+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
+
 bool initializing = true;                              // conditie voor het initialiseren
 
 
@@ -118,36 +123,29 @@
 
 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
 
-// De program starter ------------------------------------------------
-void start_program(){
-    states = init_m1;
-    pc.printf("Starting\n\r");
-}
 
-
-// De emergency break ------------------------------------------------
-void emergency_stop(){
-    safe = false;
-    pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
-}
-
-
-// De initialiseer beeindiger knop ------------------------------------------------
-void stop_initializing(){
-    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;
     }
-    ()
-*/
+    else if(states == init_m1){
+        states = finish_init_m1;
+    }
+    else if(states == finish_init_m1){
+        states = init_m2;
+    }
+    else if(states == init_m2){
+        states = finish_init_m2;
+    }
+    else if(states == finish_init_m2){
+        states = run;
+    }
+    else{
+        states = off;
+    }
+}
+
 
 // Functie voor het switchen tussen de motors ------------------------------
 void motor_switch(){
@@ -157,8 +155,8 @@
             else{
                 motor_that_runs = motor1;
             }
+}
 
-}
 
 
 // Functie voor het vinden van de positie van motor 1 ---------------------
@@ -244,21 +242,54 @@
 void flag2_activate(){flag2=true;}   
 
 
-// De initialiseren functie ------------------------------------------------------------
+// De initialiseren functie van motor 1 ------------------------------------------------------------
 void initialise_m1(){ 
     if (flag1){
         flag1 = false;
         ledred = !ledred;
+        ledblue.write(1);
         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);
     }
 }
 
 
+// De initialiseren functie van motor 2 ------------------------------------------------------------
+void initialise_m2(){ 
+    if (flag1){
+        flag1 = false;
+        ledblue = !ledblue;
+        ledgreen.write(1);
+        PID_output_2_in = PID_controller(reference_position_motor2, get_position_m2(meter_per_count)+starting_pos_m2, error_prev_2_in, error_int_2_in, Kp_2, Kd_2, Ki_2);
+        set_motor2(PID_output_2_in);
+    }
+}
+
+
+// De functies voor het afronden van de initialisatie ----------------------------------------
+void finish_initialising_m2(){
+    encoder_motor2.reset();
+    ledblue.write(1);
+    ledred.write(0);
+    motor2_speed_pin = 0;
+}
+
+
+// De functies voor het afronden van de initialisatie ----------------------------------------
+void finish_initialising_m1(){
+    encoder_motor1.reset();
+    ledred.write(1);
+    ledgreen.write(0);
+    motor1_speed_pin = 0;
+}
+    
+
+
 // De functie die de motoren aanstuurt -------------------------------------------------
 void running_motors(){
     if (flag1){
             flag1 = false;
+            ledred.write(1);
             ledgreen = !ledgreen;
             switch (motor_that_runs){
                 case motor1 :      // In deze case draait alleen motor 1
@@ -274,6 +305,15 @@
 }
 
 
+// Dit doet de robot als hij niets doet ------------------------------------------------------
+void robot_is_off(){
+    ledgreen.write(1);
+    ledblue.write(0);
+    motor1_speed_pin = 0;
+    motor2_speed_pin = 0;
+}
+
+
 // De HIDscope debug functie ----------------------------------------------------------------
 void call_HIDscope(double input_1, double input_2, double input_3){
         if (flag2){
@@ -290,22 +330,35 @@
 {
     pc.baud(SERIALBAUD);
     
-    kill_switch.fall(&emergency_stop);      // Activeert kill switch
-    test_button.fall(&motor_switch);        // Switcht motoren
+    state_switch_button.fall(&state_changer);        // Switcht states
+    motor_switch_button.fall(&motor_switch);
+    
+    /*
     start_button.fall(&start_program);
     initializing_stopper.fall(&stop_initializing);
+    */
     
     
     test_ticker.attach(&flag1_activate,0.1);                  // Activeert de go-flag van motor positie 
     hidscope_ticker.attach(&flag2_activate,0.01);   
     
-    while(safe){
+    while(1){
         switch(states){
-            case off :              // Dan staat het programma stil en gebeurt er niks
+            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 :
+                finish_initialising_m1();
+                break;
+            case init_m2 :
+                initialise_m2();
+                break;
+            case finish_init_m2 :
+                finish_initialising_m2();
+                break;  
             case run :
                 running_motors();
                 break;
@@ -313,8 +366,6 @@
         }
         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;
 }