Diff: main.cpp

Revision:

10:755b9228cc42

Parent:

9:f735baee0c2b

Child:

11:91613b22bc00

--- a/main.cpp	Mon Oct 17 09:37:38 2016 +0000
+++ b/main.cpp	Mon Oct 17 09:52:50 2016 +0000
@@ -76,13 +76,16 @@
 const double vrijheid_m2_meter = 0.25;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
 
 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 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 error_prev = 0;                      // De initiele previous error
-double error_int = 0;                       // De initiele intergral error
+double error_prev_1 = 0;                      // De initiele previous error
+double error_int_1 = 0;                       // De initiele intergral error
+
+double error_prev_2 = 0;
+double error_int_2 = 0;
 
 bool which_motor = 0;
 
@@ -206,17 +209,17 @@
         if (flag1){
             flag1 = false;
             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);
+                double 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{
-                    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);
+                    double 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_2, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count));
+            set_scope(PID_output_2,0,0);
         }
     }