Diff: main.cpp

Revision:

5:c510ab61af28

Parent:

4:6524b198721f

Child:

6:e0857842e8cd

--- a/main.cpp	Thu Oct 13 18:34:24 2016 +0000
+++ b/main.cpp	Fri Oct 14 08:55:09 2016 +0000
@@ -69,6 +69,12 @@
 const double Kd = 1.0000000;
 const double Ki = 1.0000000;
 
+const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
+
+double reference_position_motor1 = 0.5*pi;     // Dit is de eerste positie waar de motor heen wil, dit moet dezelfde zijn als de startpositie! 
+
+
+
 
 
 
@@ -96,11 +102,17 @@
 }
 
 
-// Functie voor het vinden van de reference position van motor 1 en 2 --------------------
-double get_reference_position_m1(double aantal_rad, double aantal_meter){
+// Functie voor het vinden van de reference position van motor 1 --------------------
+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;            // Aantal rad is hoeveelheid radialen van middelpunt tot minima
-    return ref_pos * aantal_meter;           // Aantal meter is de helft van de lengte die het karretje kan bewegen
+    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){
+    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;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
 }
    
 
@@ -173,12 +185,12 @@
     while(safe){            // Draait loop zolang alles veilig is.
         if (flag1){
             flag1 = false;
-            position_motor1 = get_position_m1(rad_per_count);
-            pc.printf("%f\r\n",position_motor1);
+            reference_position_motor1 = get_reference_position_m1(vrijheid_m1_rad);
+            //pc.printf("%f\r\n",get_reference_position_m1);
         }
         if (flag2){
             flag2 = false;
-            set_scope(position_motor1);
+            set_scope(reference_position_motor1);
         }
     }