Diff: ControlleurPince/ControlleurPince.cpp

Revision:

71:5590dbe8393a

Parent:

58:02dc8328975a

--- a/ControlleurPince/ControlleurPince.cpp	Wed May 04 21:51:00 2016 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Thu May 05 03:47:05 2016 +0000
@@ -2,12 +2,27 @@
 #include "defines.h"
 #include "ControlleurPince.h"
 
-ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL):
-    RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL)
+ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12) :
+    RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL), Lax12(_Lax12), Rax12(_Rax12)
 {
     pos_r = 0;
     pos_z = 0;
     pos_l = 0;
+    
+    
+}
+
+void ControlleurPince::init()
+{
+    Lax12.setMode(0);
+    wait(0.01);
+    Rax12.setMode(0);
+    wait(0.01);
+    
+    Lax12.setMaxTorque(200);
+    wait(0.01);
+    Rax12.setMaxTorque(200);
+    wait(0.01);
 }
 
 
@@ -34,9 +49,53 @@
 {
     if(z > 0.f && z < 100.f)
     {
-        logger.printf("Move by %f \n\r",z-pos_z);
-        ZMot.mm(z-pos_z);
-        
+        ZMot.mm(z-pos_z,true);
         pos_z = z;
     }
+    
+    if(delta >= 0 || center >= -500)
+    {
+        if(delta < 0)
+            delta = 130 - pos_r - pos_l;
+        if(center < -500)
+            center = - pos_r/2 + pos_l/2;
+            
+        
+        
+        /*
+        => pos_l = center*2 + pos_r
+        delta = 130 - pos_r - center*2 - pos_r
+        => 2*pos_r = 130 - center*2 - delta
+        => pos_r = (130-delta)/2 - center
+        => pos_l = (130-delta)/2 + center
+        */
+        
+        float r = (130.f-delta)/2.f - center;
+        float l = (130.f-delta)/2.f + center;
+        
+        RMot.mm(r-pos_r,true);
+        pos_r = r;
+        
+        LMot.mm(l-pos_l,true);
+        pos_l = l;
+        
+    }
+    
+    while(! (RMot.done() && ZMot.done() && LMot.done()))
+        wait(DELAY);
+}
+
+void ControlleurPince::close()
+{
+    Lax12.setGoal(150);
+    Rax12.setGoal(150);
+    wait(0.2);
+}
+
+
+void ControlleurPince::open()
+{
+    Lax12.setGoal(100);
+    Rax12.setGoal(200);
+    wait(0.2);
 }
\ No newline at end of file