Diff: fonctions_herkulex.cpp

Revision:

3:5cc8c7dfd3b7

Parent:

1:94a29f20ca18

Child:

5:3de53c9af201

--- a/fonctions_herkulex.cpp	Tue May 07 17:27:16 2019 +0000
+++ b/fonctions_herkulex.cpp	Fri May 10 09:08:47 2019 +0000
@@ -2251,15 +2251,51 @@
 //---------------------------------------------------------------------------------------------
 void deverouillage_torque (void)  //débloquer les servomoteurs
 {
-    for(int serial=1; serial<=3; serial++) {
-        for(int id=10; id<=26; id++) {
-            clear(id, serial);
-            setTorque(id, TORQUE_ON,serial);
-        }
-    }
-    
-    
-    
+   deverouillage_torque_avant();
+   deverouillage_torque_arriere();
+   deverouillage_torque_sol();
+}
+void deverouillage_torque_avant (void)
+{
+    clear(AV_EP_G, 3);
+    clear(AV_poigne_G,3);
+    clear(AV_EP_C, 2);
+    clear(AV_poigne_C, 2);
+    clear(AV_EP_D, 1);
+    clear(AV_poigne_D,1);
+
+    setTorque(AV_EP_G, TORQUE_ON,3);
+    setTorque(AV_poigne_G, TORQUE_ON,3);
+    setTorque(AV_EP_C, TORQUE_ON,2);
+    setTorque(AV_poigne_C, TORQUE_ON,2);
+    setTorque(AV_EP_D, TORQUE_ON,1);
+    setTorque(AV_poigne_D, TORQUE_ON,1);
+}
+
+void deverouillage_torque_arriere (void)
+{
+    clear(AR_EP_G, 3);
+    clear(AR_poigne_G,3);
+    clear(AR_EP_C, 2);
+    clear(AR_poigne_C, 2);
+    clear(AR_EP_D, 1);
+    clear(AR_poigne_D,1);
+
+    setTorque(AR_EP_G, TORQUE_ON,1);
+    setTorque(AR_poigne_G, TORQUE_ON,1);
+    setTorque(AR_EP_C, TORQUE_ON,2);
+    setTorque(AR_poigne_C, TORQUE_ON,2);
+    setTorque(AR_EP_D, TORQUE_ON,3);
+    setTorque(AR_poigne_D, TORQUE_ON,3);
 }
 
 
+void deverouillage_torque_sol (void){
+    
+    clear(AR_sol,4);
+    clear(AV_sol,4);
+
+    setTorque(AR_sol, TORQUE_ON,4);
+    setTorque(AV_sol, TORQUE_ON,4);
+    
+    }