Revision 11:91613b22bc00, committed 2016-10-18

Comitter:

MBroek

Date:

Tue Oct 18 09:58:54 2016 +0000

Parent:

10:755b9228cc42

Child:

12:8aba69d8d0d0

Commit message:

Beide motoren werken, met werkende switch tussen de motoren

Changed in this revision

--- a/main.cpp	Mon Oct 17 09:52:50 2016 +0000
+++ b/main.cpp	Tue Oct 18 09:58:54 2016 +0000
@@ -54,7 +54,7 @@
 const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
 const double rad_per_count = pi/8400.0;       // Aantal rad per puls uit encoder
 
-const double radius_tandwiel = 0.008;
+const double radius_tandwiel = 1.0;
 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
@@ -75,6 +75,9 @@
 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 starting_pos_m1 = 0.5*pi;                // Startin posities van de motoren
+double starting_pos_m1 = 0;
+
 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_motor2 = 0;           
 
@@ -111,14 +114,14 @@
 // 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
-    return radpercount * counts1;      // rekent positie motor1 uit en geeft deze als output vd functie
+    return radpercount * counts1 + starting_pos_m1;      // 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){     // returned de positie van het karretje in meter
     counts2 = encoder_motor2.getPulses();               // leest aantal pulses vd encoder af
-    return meterpercount * counts2;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
+    return meterpercount * counts2 + starting_pos_m2;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
 }
 
 
@@ -209,17 +212,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_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{
-                    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);
+                    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,0,0);
+            set_scope(PID_output_2, get_reference_position_m2(vrijheid_m2_meter), get_position_m2(meter_per_count));
         }
     }