Diff: ControlleurPince/ControlleurPince.cpp

Revision:

99:1fcb088f8f36

Parent:

94:86b9bd6d5c28

--- a/ControlleurPince/ControlleurPince.cpp	Fri May 06 23:55:01 2016 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Sat May 07 02:15:23 2016 +0000
@@ -1,6 +1,16 @@
 
 #include "ControlleurPince.h"
 
+float max(float a, float b)
+{
+    return a<b?b:a;
+}
+
+float min(float a, float b)
+{
+    return a<b?a:b;
+}
+
 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)
 {
@@ -44,8 +54,9 @@
 
 void ControlleurPince::setPos(float z, float delta, float center)
 {
-    if(z > 0.f && z < 100.f)
+    if(z >= 0.f)
     {
+        z = min(130,z);
         ZMot.mm(z-pos_z,true);
         pos_z = z;
     }
@@ -53,7 +64,7 @@
     if(delta >= 0 || center >= -500)
     {
         if(delta < 0)
-            delta = 130 - pos_r - pos_l;
+            delta = 150 - pos_r - pos_l;
         if(center < -500)
             center = - pos_r/2 + pos_l/2;        
         
@@ -82,15 +93,28 @@
 
 void ControlleurPince::close()
 {
-    Lax12.setGoal(150);
+    /*Lax12.setGoal(150);
     Rax12.setGoal(150);
-    wait(2);
+    wait(2);*/
+    
+    angle(0);
 }
 
 
 void ControlleurPince::open()
 {
-    Lax12.setGoal(46);
+    /*Lax12.setGoal(46);
     Rax12.setGoal(248);
+    wait(2);*/
+    
+    angle(100);
+}
+
+void ControlleurPince::angle(int a)
+{
+    a = min(a,100);
+    
+    Lax12.setGoal(150-a);
+    Rax12.setGoal(150+a);
     wait(2);
 }
\ No newline at end of file