Diff: main.cpp

Revision:

9:f735baee0c2b

Parent:

8:919ddba2875e

Child:

10:755b9228cc42

--- a/main.cpp	Mon Oct 17 07:58:15 2016 +0000
+++ b/main.cpp	Mon Oct 17 09:37:38 2016 +0000
@@ -54,7 +54,8 @@
 const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
 const double rad_per_count = pi/8400.0;       // Aantal rad per puls uit encoder
 
-const double meter_per_count = 1;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
+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!!!
 
 double error1_int = 0.00000;                 // Initiele error integral waardes
 double error2_int = 0.00000;
@@ -67,15 +68,24 @@
 const double Kd_1 = 0.1;
 const double Ki_1 = 0.3;
 
-const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
+const double Kp_2 = 1.0000000;                    // De PID variablen voor motor 2
+const double Kd_2 = 0.1;
+const double Ki_2 = 0.3;
 
-double reference_position_motor1 = 0.5*pi;     // Dit is de eerste positie waar de motor heen wil, dit moet dezelfde zijn als de startpositie! 
+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
 
-double PID_output = 0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
+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_motor3 = 0;           
+
+double PID_output_1 = 0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
+double PID_output_2 = 0;
 
 double error_prev = 0;                      // De initiele previous error
 double error_int = 0;                       // De initiele intergral error
 
+bool which_motor = 0;
+
 
 
 
@@ -89,6 +99,12 @@
 }
 
 
+// Functie voor het switchen tussen de motors ------------------------------
+void motor_switch(){
+    which_motor = !which_motor;     // =0 activeert motor1 en =1 activeert motor2
+}
+
+
 // 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
@@ -181,6 +197,7 @@
     
     //test_button.fall(&);      // Activeert test button
     kill_switch.fall(&emergency_stop);      // Activeert kill switch
+    test_button.fall(&motor_switch);        // Switcht motoren
     
     test_ticker.attach(&flag1_activate,0.1);                  // Activeert de go-flag van motor positie 
     hidscope_ticker.attach(&flag2_activate,0.01);     
@@ -188,13 +205,18 @@
     while(safe){            // Draait loop zolang alles veilig is.
         if (flag1){
             flag1 = false;
-            PID_output = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev, error_int, Kp_1, Kd_1, Ki_1);
-            set_motor1(PID_output);
-            //pc.printf("%f\r\n",error);
+            if (!which_motor){      // als which_motor=0 gaat motor 1 lopen
+                double PID_output_1 = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev, error_int, Kp_1, Kd_1, Ki_1);
+                set_motor1(PID_output_1);
+            }
+                else{
+                    double PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter), get_position_m1(meter_per_count), error_prev, error_int, Kp_2, Kd_2, Ki_2);
+                    set_motor2(PID_output_2);
+                }
         }
         if (flag2){
             flag2 = false;
-            set_scope(PID_output, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count));
+            set_scope(PID_output_2, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count));
         }
     }