Revision 20:2fdb069ffcae, committed 2016-10-24

Comitter:

MBroek

Date:

Mon Oct 24 14:15:21 2016 +0000

Parent:

19:35f3da6c6969

Child:

21:3db3f2d56d56

Commit message:

Toegevoegd dat motor1 op dezelfde positie blijft als motor 2 wordt aangestuurd. Verder dat motor 2 meedraait met motor 1 als deze wordt aangestuurd. NOG NIET GETEST

Changed in this revision

--- a/main.cpp	Mon Oct 24 11:59:27 2016 +0000
+++ b/main.cpp	Mon Oct 24 14:15:21 2016 +0000
@@ -90,11 +90,11 @@
 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 starting_pos_m1 = 0.5*pi;                // Startin posities van de motoren
-const double starting_pos_m2 = 0.25;
+const double initial_pos_m1 = 0.5*pi;                // Startin posities van de motoren
+const double initial_pos_m2 = 0.25;
 
-double reference_position_motor1 = 0;     // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
-double reference_position_motor2 = 0;           
+double starting_position_motor1 = 0;     // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
+double starting_position_motor2 = 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;
@@ -105,6 +105,9 @@
 double error_prev_2 = 0.0;
 double error_int_2 = 0.0;
 
+double reference_pos_m2;                      // De reference positie waar de motor heen wil afhankelijk van het EMG signaal
+double reference_pos_m1;
+
 enum which_motor{motor1, motor2};               // enum van de motoren
 which_motor motor_that_runs = motor1;
 
@@ -240,7 +243,7 @@
         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);     // PID met een vaste ref pos.
+        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);
     }
 }
@@ -252,7 +255,7 @@
         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);       // PID met vaste ref pos.
+        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);
     }
 }
@@ -285,12 +288,18 @@
             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);
+                    reference_pos_m1 = get_reference_position_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);
+                    set_motor2(PID_output_2);
                     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);
+                    reference_pos_m2 = get_reference_position_m2(vrijheid_m2_meter);
+                    PID_output_2 = PID_controller(reference_pos_m2, get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
+                    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);   // Zorgt ervoor dat motor1 op de dezelfde positie blijft als motor 2 draait.
                     set_motor2(PID_output_2);
+                    set_motor1(PID_output_1);
                     break;
             }
     }