Diff: main.cpp

Revision:

45:3cb347342a0d

Parent:

44:381ecf63e6ab

Child:

46:85208ab1fa12

--- a/main.cpp	Sun May 26 10:51:35 2019 +0000
+++ b/main.cpp	Thu May 30 04:05:18 2019 +0000
@@ -16,7 +16,9 @@
 char cote=0;
 
 unsigned short distance_recalage,distance_revenir;
-unsigned short  distance_goldenium;
+unsigned short  x,y;
+
+signed short x_robot,y_robot,theta_robot;
 
 void canProcessRx(void);
 
@@ -59,8 +61,6 @@
 #endif
 
     gabarit_robot();
-
-
     pc.printf("\nLAUNCHED");
     while(1) {
         /*setTorque(doigt,TORQUE_FREE,2);
@@ -68,8 +68,7 @@
         wait_ms(10.0);*/
         canProcessRx();
         f_mesure();//dt35
-
-
+        interrupt();//dt35 seuil logique
 #ifdef ROBOT_SMALL
         automate_ventouse_presentoir_arriere();
         automate_ventouse_presentoir_avant();
@@ -146,6 +145,15 @@
 
             case GLOBAL_GAME_END:
                 EtatGameEnd = 1;
+                deverouillage_torque();
+                break;
+                
+            case ODOMETRIE_BIG_POSITION:
+            case ODOMETRIE_SMALL_POSITION:
+
+                x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+                y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
+                theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
                 break;
 
             case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
@@ -202,7 +210,7 @@
                 break;
 
 
-//--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
+///////////////////////////////////////////////////////////ACK carte pompe/////////////////////////////////////////
 
 
             case HACHEUR_STATUT_VENTOUSES:
@@ -221,13 +229,11 @@
 
 
 
-//-------------------------------------------------------------------------------------------------------------------------------------------
-
-
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
             case PRESENTOIR_AVANT:
                 fpresentoir_avant=1;
-            
+
                 break;
 
             case GABARIT_ROBOT:
@@ -242,12 +248,7 @@
 
 
 #ifdef ROBOT_SMALL
-
-
-//-------------------------------------------------------------------------Actions petit robot----------------------------------------------
-
-
-
+////////////////////////////////////////////////////////Actions petit robot////////////////////////////////////////////////////////////
             case PRESENTOIR_ARRIERE:
                 fpresentoir_arriere=1;
                 break;
@@ -268,15 +269,20 @@
 
             case ACCELERATEUR_ARRIERE:
                 faccelerateur_arriere=1;
+                distance_recalage=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+                distance_revenir=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
                 break;
 
             case GOLDENIUM_AVANT:
                 fgoldenium_avant=1;
-                distance_goldenium=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+                x=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+                y=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
                 break;
 
             case GOLDENIUM_ARRIERE:
                 fgoldenium_arriere=1;
+                 x=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+                y=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
                 break;
 
             case SOL_AVANT:
@@ -295,12 +301,10 @@
                 fsol_arriere_relache=1;
                 break;
 
-
-
-
             case ARRIERE_RELACHE:
                 farriere_relache=1;
                 break;
+                
             case RECROQUEVILLER:
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
                 recroqueviller_avant();
@@ -335,12 +339,10 @@
                 accelerateur_insertion_arriere_gauche();
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 break;
-
 #endif
 
+
 #ifdef ROBOT_BIG
-
-
             case ASCENSEUR:
                 flag_ascenseur = msgRxBuffer[FIFO_lecture].data[0];
                 break;
@@ -348,7 +350,6 @@
             case HACHEUR_ETAT_CONTACTS :
                 status_contact = msgRxBuffer[FIFO_lecture].data[0];
                 break;
-
 #endif
         }
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;