Diff: ControlleurPince/ControlleurPince.cpp

Revision:

58:02dc8328975a

Child:

71:5590dbe8393a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Wed May 04 19:55:03 2016 +0000
@@ -0,0 +1,42 @@
+
+#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)
+{
+    pos_r = 0;
+    pos_z = 0;
+    pos_l = 0;
+}
+
+
+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)
+    {
+        logger.printf("Move by %f \n\r",z-pos_z);
+        ZMot.mm(z-pos_z);
+        
+        pos_z = z;
+    }
+}
\ No newline at end of file