Diff: ControlleurPince/ControlleurPince.cpp

Revision:

92:f09f55aa992b

Child:

93:c0b040954eac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Fri May 06 15:25:21 2016 +0000
@@ -0,0 +1,101 @@
+
+#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, 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);
+}
+
+
+void ControlleurPince::home(bool r, bool z, bool l)
+{
+    if(r)
+    {
+        while(EnR==true) RMot.step(1, BAS, DELAY);
+        pos_r = 0;
+    }
+    if(z)
+    {
+        while(EnZ==true) ZMot.step(1, BAS, DELAY);
+        pos_z = 0;
+    }
+    if(l)
+    {
+        while(EnL==true) LMot.step(1, BAS, DELAY);
+        pos_l = 0;
+    }
+}
+
+void ControlleurPince::setPos(float z, float delta, float center)
+{
+    if(z > 0.f && z < 100.f)
+    {
+        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