Diff: actions_Gr.cpp

Revision:

21:04ae86f7fdc8

Parent:

20:109dbdd0230b

--- a/actions_Gr.cpp	Fri May 31 20:52:23 2019 +0000
+++ b/actions_Gr.cpp	Fri May 31 21:58:59 2019 +0000
@@ -1032,14 +1032,14 @@
             break;
 
         case envoi_instruction://envoi instruction
+            SendSpeed(VITESSE_RALENTI,ACCELERATION_RALENTI,DECELERATION_RALENTI);
+            wait_us(100);
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             wait_us(50);
             SendRawId(HACHEUR_STATUT_VENTOUSES);
             wait_us(50);
             goldenium_avant();
             char pompe = AV_CENTRE;
-            SendSpeed(VITESSE_RALENTI,ACCELERATION_RALENTI,DECELERATION_RALENTI);
-            wait_us(50);
             GoStraight(200,0,0,0);//on avance vers le goldenium
             wait_us(50);
             can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
@@ -1050,7 +1050,7 @@
             break;
 
         case attente_ack_ventouse://auotmate ack carte pompe merde, renvoi ack goldenium saisi de temps en temps
-            if (ackFinAction == ASSERVISSEMENT_ERROR_MOTEUR || ackFinAction == ASSERVISSEMENT_RECALAGE ) //((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) //dès qu'il est saisi on attend pour etre sur
+            if /*(ackFinAction == ASSERVISSEMENT_ERROR_MOTEUR || ackFinAction == ASSERVISSEMENT_RECALAGE ) */((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) //dès qu'il est saisi on attend pour etre sur
             {   
                 wait(0.1);
                 
@@ -1059,7 +1059,7 @@
                 SendRawId(0x710);//on s'arrete 
                 timeout.reset();       
                 etat = reculer;
-            } else if(timeout.read_ms()>5000  &&  0) {
+            } else if(timeout.read_ms()>6000 ) {
                 ackFinAction = 0;
                 //SendRawId(ASSERVISSEMENT_STOP);
                 SendRawId(0x711);//on s'arrete 
@@ -1071,11 +1071,13 @@
         case reculer:
             if( ackFinAction == ASSERVISSEMENT_STOP || ackFinAction == ASSERVISSEMENT_ERROR_MOTEUR || ackFinAction == ASSERVISSEMENT_STOP || 1 )
             {
+                SendRawId(0x712);//on s'arrete 
                 ackFinAction = 0;
-                GoStraight(-40,0,0,0);//on recule en x donné par la strat  
+                GoStraight(-500,0,0,0);//on recule en x donné par la strat 
+                ackFinAction = 0; 
                 wait(0.1);
-                positionControl(AV_poigne_C,450,150,BLED_ON,2);//forklift
-                fgoldenium_avant=0;
+                //positionControl(AV_poigne_C,450,150,BLED_ON,2);//forklift
+                SendSpeed(VITESSE_NORMAL,ACCELERATION,DECELERATION);
                 etat = xyt;
             }
             break;
@@ -1083,8 +1085,8 @@
         case xyt:
             if(ackFinAction == ASSERVISSEMENT_RECALAGE)
             {
+                SendRawId(0x713);//on s'arrete 
                 ackFinAction = 0;
-                SendSpeed(VITESSE_NORMAL,ACCELERATION,DECELERATION);
                 GoToPosition( x_goldenium,y_goldenium,1800,-1);//x et y donné par la strat ,theta constant,-1 marche arriere
                 etat = fin;
             }
@@ -1094,6 +1096,7 @@
             {
                 ackFinAction = 0;
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                fgoldenium_avant=0;
                 etat = init;
             }
             break;