Diff: main.cpp

Revision:

0:bc74da1c502f

Child:

1:568955af8c2b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 06 11:18:47 2019 +0000
@@ -0,0 +1,224 @@
+#include "mbed.h"
+#include "ident_crac.h"
+#include "Capteur.h"
+#include "Actionneurs.h"
+#include "fonctions_herkulex.h"
+
+#define SIZE_FIFO 50
+
+CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
+
+
+CANMessage msgRxBuffer[SIZE_FIFO];
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+unsigned char EtatGameEnd=0;
+
+void canRx_ISR (void);
+void SendAck(unsigned short id, unsigned short from);
+void SendRawId (unsigned short id);
+void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
+void canProcessRx(void);
+
+/*********************************************************************************************/
+/* FUNCTION NAME: canRx_ISR                                                                  */
+/* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                     */
+/*********************************************************************************************/
+void canRx_ISR (void)
+{
+    if (can.read(msgRxBuffer[FIFO_ecriture])) {
+        if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
+
+        }
+        FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+
+    }
+
+}
+/*********************************************************************************************/
+/* FUNCTION NAME: SendAck                                                                    */
+/* DESCRIPTION  :                                                                            */
+/*********************************************************************************************/
+void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=from;                              //waitingAckFrom
+    msgTx.len=2;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // from sur 2 octets
+    msgTx.data[0]=(unsigned char)id;            //waitingAckID
+    msgTx.data[1]=(unsigned char)(id>>8);
+    can.write(msgTx);
+}
+/*********************************************************************************************/
+/* FUNCTION NAME: SendRawId                                                                  */
+/* DESCRIPTION  :                                                                            */
+/*********************************************************************************************/
+void SendRawId (unsigned short id)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=0;
+    can.write(msgTx); //carte esclave f446re
+    wait_us(200);
+}
+
+/*********************************************************************************************/
+/* FUNCTION NAME: GoStraight                                                                 */
+/* DESCRIPTION  : Transmission CAN correspondant à une ligne droite, avec ou sans recalage   */
+/*  recalage : 0 => pas de recalage                                                          */
+/*             1 => recalage en X                                                            */
+/*             2 => Recalage en Y                                                            */
+/*  newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo            */
+/*  isEnchainement : Indique si il faut executer l'instruction en enchainement               */
+/*                   0 => non                                                                */
+/*                   1 => oui                                                                */
+/*                   2 => dernière instruction de l'enchainement                             */
+/*********************************************************************************************/
+void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_RECALAGE;
+    msgTx.len=6;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    // x sur 2 octets
+    msgTx.data[0]=(unsigned char)distance;
+    msgTx.data[1]=(unsigned char)(distance>>8);
+    //Recalage sur 1 octet
+    msgTx.data[2]=recalage;
+    //Valeur du recalage sur 2 octets
+    msgTx.data[3]=(unsigned char)newValue;
+    msgTx.data[4]=(unsigned char)(newValue>>8);
+    //Enchainement sur 1 octet
+    msgTx.data[5]=isEnchainement;
+
+    can.write(msgTx);
+    //wait_ms(500);
+}
+
+int main()
+{
+    can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+    servo_interrupt_en(); //permettre les interuptions
+    wait(1);//attente servo boot
+    init_petit_robot();
+
+    while(1) {
+        canProcessRx();
+
+        init_petit_robot();
+
+        presentoir_avant();
+        presentoir_arriere();
+
+        balance_avant();
+        balance_arriere();
+
+        if(EtatGameEnd==1) {
+            while(1);
+        }
+
+    }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx                                                          */
+/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
+/****************************************************************************************/
+void canProcessRx(void)
+{
+    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+    CANMessage msgTx=CANMessage();
+    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+    if(FIFO_occupation<0)
+        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+    if(FIFO_max_occupation<FIFO_occupation)
+        FIFO_max_occupation=FIFO_occupation;
+    if(FIFO_occupation!=0) {
+        int identifiant=msgRxBuffer[FIFO_lecture].id;
+
+        switch(identifiant) {
+
+            case GLOBAL_GAME_END:
+                EtatGameEnd = 1;
+                break;
+
+            case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
+                char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
+                short distance=lecture_telemetre(numero_telemetre);
+
+
+                msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
+                msgTx.len=2;
+                msgTx.format=CANStandard;
+                msgTx.type=CANData;
+                // Rayon sur 2 octets
+                msgTx.data[0]=(unsigned char)distance;
+                msgTx.data[1]=(unsigned char)(distance>>8);
+
+                can.write(msgTx);
+                SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
+                break;
+
+            case DATA_RECALAGE:
+                short distance1=lecture_telemetre(1);
+                short distance2=lecture_telemetre(2);
+                short distance3=lecture_telemetre(3);
+                short distance4=lecture_telemetre(4);
+
+                msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
+                msgTx.len=8;
+                msgTx.format=CANStandard;
+                msgTx.type=CANData;
+                // Rayon sur 2 octets
+                msgTx.data[0]=(unsigned char)distance1;
+                msgTx.data[1]=(unsigned char)(distance1>>8);
+                msgTx.data[2]=(unsigned char)distance2;
+                msgTx.data[3]=(unsigned char)(distance2>>8);
+                msgTx.data[4]=(unsigned char)distance3;
+                msgTx.data[5]=(unsigned char)(distance3>>8);
+                msgTx.data[6]=(unsigned char)distance4;
+                msgTx.data[7]=(unsigned char)(distance4>>8);
+                can.write(msgTx);
+                SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
+                break;
+
+#ifdef ROBOT_SMALL
+            case INIT_PETIT_ROBOT:
+                init_petit_robot();
+                break;
+
+            case PRESENTOIR_AVANT:
+                presentoir_avant();
+                break;
+
+            case PRESENTOIR_ARRIERE:
+                presentoir_arriere();
+                break;
+
+            case BALANCE_AVANT:
+                balance_avant();
+                break;
+
+            case BALANCE_ARRIERE:
+                balance_arriere();
+                break;
+
+#endif
+#ifdef ROBOT_BIG
+            case 3:
+
+                break;
+
+            case 5:
+
+                break;
+
+#endif
+        }
+        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+
+    }
+}
\ No newline at end of file