Diff: Actionneurs/herkulex_rob.cpp

Revision:

23:93a427b242ce

Child:

25:9627700d989f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/herkulex_rob.cpp	Wed May 22 20:07:49 2019 +0000
@@ -0,0 +1,123 @@
+#include "main.h"
+#include "herkulex_rob.h"
+
+
+//---------------------------------------Petit robot---------------------------------------------------------
+/*void gros_robot_init(void)
+{
+void   palet_accelerateur(){
+    compteTour(ID,1000,1,1,PLED_ON,serialbarillet);
+    }
+void palet_balance(){
+    compteTour(ID,-1000,1,1,PLED_ON,serialbarillet);
+    }
+void sortie_balance()
+    {
+
+    }
+
+    */
+
+
+#ifdef ROBOT_SMALL
+//---------------------------------------------------------------------------------------------
+void deverouillage_torque (void)  //débloquer les servomoteurs
+{
+    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);
+
+}
+
+#endif 
+
+#ifdef ROBOT_BIG
+
+void deverouillage_torque (void)  //débloquer les servomoteurs
+{
+    deverouillage_torque_bras();
+    deverouillage_torque_convoyeurs();
+    deverouillage_torque_doight();
+    deverouillage_torque_roues();
+}
+
+
+void deverouillage_torque_bras(void)
+{
+    clear(AV_EP_C,2);
+    clear(AV_poigne_C,2);
+
+    setTorque(AV_EP_C, TORQUE_ON,2);
+    setTorque(AV_poigne_C, TORQUE_ON,2);
+}
+
+void deverouillage_torque_convoyeurs(void)
+{
+    clear(stockage_D,3);
+    clear(stockage_G,1);
+
+    setTorque(stockage_D, TORQUE_ON,3);
+    setTorque(stockage_G, TORQUE_ON,1);
+}
+
+void deverouillage_torque_roues(void)
+{
+    clear(roue_D,3);
+    clear(roue_G,1);
+
+    setTorque(roue_D, TORQUE_ON,3);
+    setTorque(roue_G, TORQUE_ON,1);
+}
+
+
+void deverouillage_torque_doigt(void)
+{
+    clear(doigt,2);
+    setTorque(doigt, TORQUE_ON,2);
+}
+
+#endif