Diff: main.cpp

Revision:

13:b0d3c547bf2f

Parent:

12:8aba69d8d0d0

Child:

14:f51d51395803

--- a/main.cpp	Wed Oct 19 10:30:32 2016 +0000
+++ b/main.cpp	Fri Oct 21 08:44:18 2016 +0000
@@ -35,6 +35,12 @@
 AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
 AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2
 
+InterruptIn start_button(SW2);
+InterruptIn initializing_stopper(SW3);
+
+DigitalOut ledred(LED_RED, 1);
+DigitalOut ledgreen(LED_GREEN, 1);
+
 
 //Definieren van de tickers ------------------------------------------
 Ticker test_ticker;
@@ -44,6 +50,14 @@
 
 // HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
 
+bool starting = false;                               // conditie om programma te starten
+
+
+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
+bool initializing = true;                              // conditie voor het initialiseren
+
+
 volatile bool safe = true;                      // Conditie voor de while-loop in main
 
 double position_motor1;
@@ -76,21 +90,21 @@
 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 starting_pos_m1 = 0.5*pi;                // Startin posities van de motoren
-const double starting_pos_m2 = 0;
+const double starting_pos_m2 = 0.25;
 
-double reference_position_motor1 = 0.5*pi;     // Dit zijn de eerste posities waar de motoren heen willen, deze moeten dezelfde zijn als de startpositie!
+double reference_position_motor1 = 0;     // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
 double reference_position_motor2 = 0;           
 
-double PID_output_1 = 0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
-double PID_output_2 = 0;
+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 error_prev_1 = 0;                      // De initiele previous error
-double error_int_1 = 0;                       // De initiele intergral error
+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;
-double error_int_2 = 0;
+double error_prev_2 = 0.0;
+double error_int_2 = 0.0;
 
-bool which_motor = 0;
+bool which_motor = false;
 
 
 
@@ -98,6 +112,13 @@
 
 // ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================
 
+// De program starter ------------------------------------------------
+void start_program(){
+    starting = true;
+    pc.printf("Starting\n\r");
+}
+
+
 // De emergency break ------------------------------------------------
 void emergency_stop(){
     safe = false;
@@ -105,6 +126,13 @@
 }
 
 
+// De initialiseer beeindiger knop ------------------------------------------------
+void stop_initializing(){
+    initializing = false;
+    pc.printf("Initializing ended\r\n");
+}
+
+
 // Functie voor het switchen tussen de motors ------------------------------
 void motor_switch(){
     which_motor = !which_motor;     // =0 activeert motor1 en =1 activeert motor2
@@ -112,30 +140,30 @@
 
 
 // Functie voor het vinden van de positie van motor 1 ---------------------
-double get_position_m1(const double radpercount, const double start_pos_m1){          //returned de positie van de motor in radialen (positie vd arm in radialen)
+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 + start_pos_m1;      // rekent positie motor1 uit en geeft deze als output vd functie
+    return radpercount * counts1;      // 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, const double start_pos_m2){     // returned de positie van het karretje in meter
+double get_position_m2(const double meterpercount){     // returned de positie van het karretje in meter
     counts2 = encoder_motor2.getPulses();               // leest aantal pulses vd encoder af
-    return meterpercount * counts2 + start_pos_m2;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
+    return meterpercount * counts2;                     // 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(const double aantal_rad, const double start_pos_m1){
+double get_reference_position_m1(const double aantal_rad){
     double ref_pos = pot1.read() - pot2.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 + start_pos_m1;            // Aantal rad is de uiterste positie vd arm in radialen                                                                    
+    return ref_pos * aantal_rad;            // Aantal 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, const double start_pos_m2){
+double get_reference_position_m2(const double aantal_meter){
     double ref_pos = pot1.read() - pot2.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 + start_pos_m2;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
+    return ref_pos * aantal_meter;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
 }
    
 
@@ -199,36 +227,56 @@
 int main()
 {
     pc.baud(SERIALBAUD);
-    pc.printf("Starting");
     
-    //test_button.fall(&);      // Activeert test button
     kill_switch.fall(&emergency_stop);      // Activeert kill switch
     test_button.fall(&motor_switch);        // Switcht motoren
+    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);     
+    hidscope_ticker.attach(&flag2_activate,0.01);   
+    
+    while(1){
+    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, starting_pos_m1), get_position_m1(rad_per_count, starting_pos_m1), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_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);
             }
                 else{
-                    PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter, starting_pos_m2), get_position_m2(meter_per_count, starting_pos_m2), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
+                    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, starting_pos_m1), get_position_m1(meter_per_count, starting_pos_m1));
+            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;
 }
+}
+}