Revision 9:d0042422d95a, committed 2016-04-28

Comitter:

antbig

Date:

Thu Apr 28 08:11:36 2016 +0000

Parent:

8:0edc7dfb7f7e

Child:

10:a788d9cf60f2

Commit message:

ajout message choix id strat

Changed in this revision

--- a/Asservissement/Asservissement.cpp	Wed Apr 27 13:05:03 2016 +0000
+++ b/Asservissement/Asservissement.cpp	Thu Apr 28 08:11:36 2016 +0000
@@ -134,3 +134,24 @@
 
     can1.write(msgTx);
 }
+
+void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=canId;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=6;
+
+        // x sur 2 octets
+    msgTx.data[0]=(unsigned char)x;
+    msgTx.data[1]=(unsigned char)(x>>8);
+    // y sur 2 octets
+    msgTx.data[2]=(unsigned char)y;
+    msgTx.data[3]=(unsigned char)(y>>8);
+    // theta signé sur 2 octets
+    msgTx.data[4]=(unsigned char)theta;
+    msgTx.data[5]=(unsigned char)(theta>>8);
+
+    can1.write(msgTx);
+}

--- a/Asservissement/Asservissement.h	Wed Apr 27 13:05:03 2016 +0000
+++ b/Asservissement/Asservissement.h	Thu Apr 28 08:11:36 2016 +0000
@@ -49,4 +49,6 @@
 /********************************************************************************************/
 void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement);
 
+void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta);
+
 #endif
\ No newline at end of file

--- a/Globals/constantes.h	Wed Apr 27 13:05:03 2016 +0000
+++ b/Globals/constantes.h	Thu Apr 28 08:11:36 2016 +0000
@@ -16,17 +16,21 @@
 /****
 ** Variable à modifier en fonction du robot
 ***/
-#define ROBOT_BIG//Indique que l'on va compiler pour le gros robot
-//#define ROBOT_SMALL
+//#define ROBOT_BIG//Indique que l'on va compiler pour le gros robot
+#define ROBOT_SMALL
 
 #ifdef ROBOT_BIG
 
     #define NOMBRE_CARTES           2 //Le nombre de carte présente sur le robot
-
+    #define POSITION_DEBUT_X 765
+    #define POSITION_DEBUT_Y 100
+    #define POSITION_DEBUT_T 900
 #else
 
     #define NOMBRE_CARTES           2 //Le nombre de carte présente sur le robot
-
+    #define POSITION_DEBUT_X 990
+    #define POSITION_DEBUT_Y 150
+    #define POSITION_DEBUT_T 0
 #endif
 
 

--- a/Robots/Config_big.h	Wed Apr 27 13:05:03 2016 +0000
+++ b/Robots/Config_big.h	Thu Apr 28 08:11:36 2016 +0000
@@ -6,7 +6,12 @@
 
 #define AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE  18
 #define AX12_ID_PINCE_ARRIERE_HAUTE_DROITE  15
+#define AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE  13
+#define AX12_ID_PINCE_ARRIERE_BASSE_DROITE  14
 #define AX12_ID_PORTE_ARRIERE_DROITE        4
 #define AX12_ID_PORTE_ARRIERE_GAUCHE        5
+#define AX12_ID_FUNNY_ACTION                1
+
+
 
 #endif
\ No newline at end of file

--- a/Robots/Config_small.h	Wed Apr 27 13:05:03 2016 +0000
+++ b/Robots/Config_small.h	Thu Apr 28 08:11:36 2016 +0000
@@ -9,5 +9,4 @@
 #define AX12_ID_BRAS_RELACHEUR_INV      2
 
 
-
 #endif
\ No newline at end of file

--- a/Robots/StrategieManager.h	Wed Apr 27 13:05:03 2016 +0000
+++ b/Robots/StrategieManager.h	Thu Apr 28 08:11:36 2016 +0000
@@ -22,6 +22,12 @@
 void initRobot(void);
 
 /****************************************************************************************/
+/* FUNCTION NAME: runTest                                                               */
+/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
+/****************************************************************************************/
+void runRobotTest(void);
+
+/****************************************************************************************/
 /* FUNCTION NAME: SelectStrategy                                                        */
 /* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
 /* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */

--- a/Robots/Strategie_big.cpp	Wed Apr 27 13:05:03 2016 +0000
+++ b/Robots/Strategie_big.cpp	Thu Apr 28 08:11:36 2016 +0000
@@ -7,7 +7,9 @@
 /* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
 /****************************************************************************************/
 void doFunnyAction(void) {
-    
+    AX12_setGoal(AX12_ID_FUNNY_ACTION, 275);
+    AX12_setGoal(AX12_ID_FUNNY_ACTION, 180);
+    AX12_processChange();
     
 }
 
@@ -17,9 +19,7 @@
 /****************************************************************************************/
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
     switch(id) {
-        case 101:
         
-        break;
         case 110://Ouvrir la pince arrière haute
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
@@ -30,12 +30,18 @@
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
             AX12_processChange();
         break;
+        
         case 112://Ouvrir la pince arrière basse
-        
+            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 205);
+            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 95);
+            AX12_processChange();
         break;
         case 113://Fermer la pince arrière basse
+            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
+            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
+            AX12_processChange();
+        break;
         
-        break;
         case 114://Ouvrir les portes arrières
             AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
             AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
@@ -58,13 +64,38 @@
 /* FUNCTION NAME: initRobot                                                             */
 /* DESCRIPTION  : initialiser le robot                                                  */
 /****************************************************************************************/
-void initRobot(void) {
+void initRobot(void) 
+{
+    //Enregistrement de tous les AX12 présent sur la carte
     AX12_register(4,  AX12_SERIAL1);
     AX12_register(14, AX12_SERIAL1);
     AX12_register(15, AX12_SERIAL1);
     AX12_register(5,  AX12_SERIAL2);
     AX12_register(18, AX12_SERIAL2);
     AX12_register(13, AX12_SERIAL2);
+    AX12_register(1,  AX12_SERIAL2);
+    
+    runRobotTest();
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: runTest                                                               */
+/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
+/****************************************************************************************/
+void runRobotTest(void) 
+{
+    //Test des AX12 dans l'ordre
+    doAction(111,0,0);//Fermeture pince arrière haute
+    wait_ms(500);
+    doAction(110,0,0);//Ouverture pince arrière haute
+    wait_ms(500);
+    doAction(113,0,0);//Fermeture pince arrière basse
+    wait_ms(500);
+    doAction(112,0,0);//Ouverture pince arrière basse
+    wait_ms(500);
+    doAction(115,0,0);//Fermeture porte arrière
+    wait_ms(500);
+    doAction(114,0,0);//Ouverture porte arrière
 }
 
 /****************************************************************************************/

--- a/Robots/Strategie_small.cpp	Wed Apr 27 13:05:03 2016 +0000
+++ b/Robots/Strategie_small.cpp	Thu Apr 28 08:11:36 2016 +0000
@@ -17,7 +17,7 @@
 /****************************************************************************************/
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
     switch(id) {
-        case 101://Descendre le bras pour les poissons
+        /*case 101://Descendre le bras pour les poissons
             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche
                 AX12_setGoal(AX12_ID_BRAS_BASE_INV,200,0x0FF);
                 AX12_processChange();
@@ -62,12 +62,14 @@
         case 106:
             //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
             //AX12_processChange();
-        break;
+        break;*/
         default:
-            return 0;//L'action n'existe pas, il faut utiliser le CAN
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            return 1;//L'action n'existe pas, il faut utiliser le CAN
         
     }
-    return 1;//L'action est spécifique.
+    //return 1;//L'action est spécifique.
     
 }
 
@@ -94,6 +96,15 @@
 }
 
 /****************************************************************************************/
+/* FUNCTION NAME: runTest                                                               */
+/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
+/****************************************************************************************/
+void runRobotTest(void) 
+{
+    
+}
+
+/****************************************************************************************/
 /* FUNCTION NAME: SelectStrategy                                                        */
 /* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
 /* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */

--- a/Strategie/Strategie.cpp	Wed Apr 27 13:05:03 2016 +0000
+++ b/Strategie/Strategie.cpp	Thu Apr 28 08:11:36 2016 +0000
@@ -204,6 +204,12 @@
             SendRawId(ECRAN_ACK_START_MATCH);
             tactile_printf("Attente du JACK.");
             jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
+            
+#ifdef ROBOT_BIG
+            SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
+#else
+            SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
+#endif
         break;
         case ETAT_GAME_WAIT_FOR_JACK:
             //TODO Attendre le jack
@@ -255,7 +261,7 @@
                     }
                     
                     localData1 = ((instruction.direction == FORWARD)?1:-1);
-                    BendRadius (instruction.arg1, instruction.arg3, localData1, localData5);
+                    BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
                 break;
                 case MV_LINE:
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
@@ -388,8 +394,8 @@
                             waitingAckFrom = INSTRUCTION_END_MOTEUR;
                         break;
                         case ACTION:
-                            waitingAckFrom = SERVO_AX12_DONE;
-                            waitingAckID = instruction.arg1;
+                            waitingAckFrom = 0;//SERVO_AX12_DONE;
+                            waitingAckID = 0;//instruction.arg1;
                         break;
                         default:
                         break;
@@ -517,17 +523,17 @@
                 }
             break;
             case SERVO_AX12_SETGOAL:
-                SendAck(0x114, SERVO_AX12_SETGOAL);
+                //SendAck(0x114, SERVO_AX12_SETGOAL);
                 if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
                     AX12_setGoal(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[2])<<8), msgRxBuffer[FIFO_lecture].data[3]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[4])<<8));
                   
             break;
             case SERVO_AX12_PROCESS:
-                SendAck(0x114, SERVO_AX12_PROCESS);
+                //SendAck(0x114, SERVO_AX12_PROCESS);
                 AX12_processChange(1);
             break;
             case SERVO_AX12_DONE:
-                SendAck(0x114, SERVO_AX12_DONE);
+                //SendAck(0x114, SERVO_AX12_DONE);
                 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
             break;
             case ECRAN_CHOICE_COLOR://Choix de la couleur
@@ -544,6 +550,7 @@
                     // couleur sur 1 octet
                     msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
                     can1.write(msgTx);
+                    
                 }
             break;
             case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
@@ -560,6 +567,8 @@
                         msgTx.data[0]=0;
                     }
                     can1.write(msgTx);
+                    wait_ms(10);
+                    tactile_printf("Strat %d selectionne",msgTx.data[0]);
                 }
             break;
             case BALISE_STOP:

--- a/main.cpp	Wed Apr 27 13:05:03 2016 +0000
+++ b/main.cpp	Thu Apr 28 08:11:36 2016 +0000
@@ -32,11 +32,11 @@
     can1.frequency(1000000); // fréquence de travail 1Mbit/s
     can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
     
-    //wait_ms(1000);
-    //tactile_printf("Initialisation cartes...");
+    wait_ms(1000);
+    tactile_printf("Initialisation cartes...");
     initRobot();//Initialisation du robot
     
-    //wait_ms(5000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
+    wait_ms(5000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
     
     /**
     A retirer lors de l'utilisation avec selecteur de stratégie sur IHM
@@ -45,7 +45,7 @@
     //loadAllInstruction();//Mise en cache de toute les instructions
     
     while(true) {
-        //automate_process();//Boucle dans l'automate principal
+        automate_process();//Boucle dans l'automate principal
         canProcessRx();//Traitement des trames CAN en attente  
         AX12_doLoop();//Vérification de la position des AX12
     }