CRAC Team / Mbed 2 deprecated carte_esclave201_petit_rob

carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp@0:bc74da1c502f, 2019-05-06 (annotated)

Committer:
Artiom
Date:
Mon May 06 11:18:47 2019 +0000
Revision:
0:bc74da1c502f
Child:
1:568955af8c2b
Z

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:bc74da1c502f 1 #include "mbed.h"
Artiom 0:bc74da1c502f 2 #include "ident_crac.h"
Artiom 0:bc74da1c502f 3 #include "Capteur.h"
Artiom 0:bc74da1c502f 4 #include "Actionneurs.h"
Artiom 0:bc74da1c502f 5 #include "fonctions_herkulex.h"
Artiom 0:bc74da1c502f 6
Artiom 0:bc74da1c502f 7 #define SIZE_FIFO 50
Artiom 0:bc74da1c502f 8
Artiom 0:bc74da1c502f 9 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom 0:bc74da1c502f 10
Artiom 0:bc74da1c502f 11
Artiom 0:bc74da1c502f 12 CANMessage msgRxBuffer[SIZE_FIFO];
Artiom 0:bc74da1c502f 13 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom 0:bc74da1c502f 14 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom 0:bc74da1c502f 15 unsigned char EtatGameEnd=0;
Artiom 0:bc74da1c502f 16
Artiom 0:bc74da1c502f 17 void canRx_ISR (void);
Artiom 0:bc74da1c502f 18 void SendAck(unsigned short id, unsigned short from);
Artiom 0:bc74da1c502f 19 void SendRawId (unsigned short id);
Artiom 0:bc74da1c502f 20 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
Artiom 0:bc74da1c502f 21 void canProcessRx(void);
Artiom 0:bc74da1c502f 22
Artiom 0:bc74da1c502f 23 /*********************************************************************************************/
Artiom 0:bc74da1c502f 24 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 25 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 26 /*********************************************************************************************/
Artiom 0:bc74da1c502f 27 void canRx_ISR (void)
Artiom 0:bc74da1c502f 28 {
Artiom 0:bc74da1c502f 29 if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom 0:bc74da1c502f 30 if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom 0:bc74da1c502f 31
Artiom 0:bc74da1c502f 32 }
Artiom 0:bc74da1c502f 33 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 34
Artiom 0:bc74da1c502f 35 }
Artiom 0:bc74da1c502f 36
Artiom 0:bc74da1c502f 37 }
Artiom 0:bc74da1c502f 38 /*********************************************************************************************/
Artiom 0:bc74da1c502f 39 /* FUNCTION NAME: SendAck */
Artiom 0:bc74da1c502f 40 /* DESCRIPTION : */
Artiom 0:bc74da1c502f 41 /*********************************************************************************************/
Artiom 0:bc74da1c502f 42 void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale
Artiom 0:bc74da1c502f 43 {
Artiom 0:bc74da1c502f 44 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 45 msgTx.id=from; //waitingAckFrom
Artiom 0:bc74da1c502f 46 msgTx.len=2;
Artiom 0:bc74da1c502f 47 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 48 msgTx.type=CANData;
Artiom 0:bc74da1c502f 49 // from sur 2 octets
Artiom 0:bc74da1c502f 50 msgTx.data[0]=(unsigned char)id; //waitingAckID
Artiom 0:bc74da1c502f 51 msgTx.data[1]=(unsigned char)(id>>8);
Artiom 0:bc74da1c502f 52 can.write(msgTx);
Artiom 0:bc74da1c502f 53 }
Artiom 0:bc74da1c502f 54 /*********************************************************************************************/
Artiom 0:bc74da1c502f 55 /* FUNCTION NAME: SendRawId */
Artiom 0:bc74da1c502f 56 /* DESCRIPTION : */
Artiom 0:bc74da1c502f 57 /*********************************************************************************************/
Artiom 0:bc74da1c502f 58 void SendRawId (unsigned short id)
Artiom 0:bc74da1c502f 59 {
Artiom 0:bc74da1c502f 60 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 61 msgTx.id=id;
Artiom 0:bc74da1c502f 62 msgTx.len=0;
Artiom 0:bc74da1c502f 63 can.write(msgTx); //carte esclave f446re
Artiom 0:bc74da1c502f 64 wait_us(200);
Artiom 0:bc74da1c502f 65 }
Artiom 0:bc74da1c502f 66
Artiom 0:bc74da1c502f 67 /*********************************************************************************************/
Artiom 0:bc74da1c502f 68 /* FUNCTION NAME: GoStraight */
Artiom 0:bc74da1c502f 69 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
Artiom 0:bc74da1c502f 70 /* recalage : 0 => pas de recalage */
Artiom 0:bc74da1c502f 71 /* 1 => recalage en X */
Artiom 0:bc74da1c502f 72 /* 2 => Recalage en Y */
Artiom 0:bc74da1c502f 73 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
Artiom 0:bc74da1c502f 74 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
Artiom 0:bc74da1c502f 75 /* 0 => non */
Artiom 0:bc74da1c502f 76 /* 1 => oui */
Artiom 0:bc74da1c502f 77 /* 2 => dernière instruction de l'enchainement */
Artiom 0:bc74da1c502f 78 /*********************************************************************************************/
Artiom 0:bc74da1c502f 79 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
Artiom 0:bc74da1c502f 80 {
Artiom 0:bc74da1c502f 81 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 82 msgTx.id=ASSERVISSEMENT_RECALAGE;
Artiom 0:bc74da1c502f 83 msgTx.len=6;
Artiom 0:bc74da1c502f 84 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 85 msgTx.type=CANData;
Artiom 0:bc74da1c502f 86 // x sur 2 octets
Artiom 0:bc74da1c502f 87 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 88 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 89 //Recalage sur 1 octet
Artiom 0:bc74da1c502f 90 msgTx.data[2]=recalage;
Artiom 0:bc74da1c502f 91 //Valeur du recalage sur 2 octets
Artiom 0:bc74da1c502f 92 msgTx.data[3]=(unsigned char)newValue;
Artiom 0:bc74da1c502f 93 msgTx.data[4]=(unsigned char)(newValue>>8);
Artiom 0:bc74da1c502f 94 //Enchainement sur 1 octet
Artiom 0:bc74da1c502f 95 msgTx.data[5]=isEnchainement;
Artiom 0:bc74da1c502f 96
Artiom 0:bc74da1c502f 97 can.write(msgTx);
Artiom 0:bc74da1c502f 98 //wait_ms(500);
Artiom 0:bc74da1c502f 99 }
Artiom 0:bc74da1c502f 100
Artiom 0:bc74da1c502f 101 int main()
Artiom 0:bc74da1c502f 102 {
Artiom 0:bc74da1c502f 103 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 104 servo_interrupt_en(); //permettre les interuptions
Artiom 0:bc74da1c502f 105 wait(1);//attente servo boot
Artiom 0:bc74da1c502f 106 init_petit_robot();
Artiom 0:bc74da1c502f 107
Artiom 0:bc74da1c502f 108 while(1) {
Artiom 0:bc74da1c502f 109 canProcessRx();
Artiom 0:bc74da1c502f 110
Artiom 0:bc74da1c502f 111 init_petit_robot();
Artiom 0:bc74da1c502f 112
Artiom 0:bc74da1c502f 113 presentoir_avant();
Artiom 0:bc74da1c502f 114 presentoir_arriere();
Artiom 0:bc74da1c502f 115
Artiom 0:bc74da1c502f 116 balance_avant();
Artiom 0:bc74da1c502f 117 balance_arriere();
Artiom 0:bc74da1c502f 118
Artiom 0:bc74da1c502f 119 if(EtatGameEnd==1) {
Artiom 0:bc74da1c502f 120 while(1);
Artiom 0:bc74da1c502f 121 }
Artiom 0:bc74da1c502f 122
Artiom 0:bc74da1c502f 123 }
Artiom 0:bc74da1c502f 124 }
Artiom 0:bc74da1c502f 125
Artiom 0:bc74da1c502f 126 /****************************************************************************************/
Artiom 0:bc74da1c502f 127 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 128 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 129 /****************************************************************************************/
Artiom 0:bc74da1c502f 130 void canProcessRx(void)
Artiom 0:bc74da1c502f 131 {
Artiom 0:bc74da1c502f 132 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 133 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 134 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 135 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 136 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 0:bc74da1c502f 137 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 138 FIFO_max_occupation=FIFO_occupation;
Artiom 0:bc74da1c502f 139 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 140 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 141
Artiom 0:bc74da1c502f 142 switch(identifiant) {
Artiom 0:bc74da1c502f 143
Artiom 0:bc74da1c502f 144 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 145 EtatGameEnd = 1;
Artiom 0:bc74da1c502f 146 break;
Artiom 0:bc74da1c502f 147
Artiom 0:bc74da1c502f 148 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 149 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 150 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 151
Artiom 0:bc74da1c502f 152
Artiom 0:bc74da1c502f 153 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 154 msgTx.len=2;
Artiom 0:bc74da1c502f 155 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 156 msgTx.type=CANData;
Artiom 0:bc74da1c502f 157 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 158 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 159 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 160
Artiom 0:bc74da1c502f 161 can.write(msgTx);
Artiom 0:bc74da1c502f 162 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 163 break;
Artiom 0:bc74da1c502f 164
Artiom 0:bc74da1c502f 165 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 166 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 167 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 168 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 169 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 170
Artiom 0:bc74da1c502f 171 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 172 msgTx.len=8;
Artiom 0:bc74da1c502f 173 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 174 msgTx.type=CANData;
Artiom 0:bc74da1c502f 175 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 176 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 177 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 178 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 179 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 180 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 181 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 182 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 183 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 184 can.write(msgTx);
Artiom 0:bc74da1c502f 185 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 186 break;
Artiom 0:bc74da1c502f 187
Artiom 0:bc74da1c502f 188 #ifdef ROBOT_SMALL
Artiom 0:bc74da1c502f 189 case INIT_PETIT_ROBOT:
Artiom 0:bc74da1c502f 190 init_petit_robot();
Artiom 0:bc74da1c502f 191 break;
Artiom 0:bc74da1c502f 192
Artiom 0:bc74da1c502f 193 case PRESENTOIR_AVANT:
Artiom 0:bc74da1c502f 194 presentoir_avant();
Artiom 0:bc74da1c502f 195 break;
Artiom 0:bc74da1c502f 196
Artiom 0:bc74da1c502f 197 case PRESENTOIR_ARRIERE:
Artiom 0:bc74da1c502f 198 presentoir_arriere();
Artiom 0:bc74da1c502f 199 break;
Artiom 0:bc74da1c502f 200
Artiom 0:bc74da1c502f 201 case BALANCE_AVANT:
Artiom 0:bc74da1c502f 202 balance_avant();
Artiom 0:bc74da1c502f 203 break;
Artiom 0:bc74da1c502f 204
Artiom 0:bc74da1c502f 205 case BALANCE_ARRIERE:
Artiom 0:bc74da1c502f 206 balance_arriere();
Artiom 0:bc74da1c502f 207 break;
Artiom 0:bc74da1c502f 208
Artiom 0:bc74da1c502f 209 #endif
Artiom 0:bc74da1c502f 210 #ifdef ROBOT_BIG
Artiom 0:bc74da1c502f 211 case 3:
Artiom 0:bc74da1c502f 212
Artiom 0:bc74da1c502f 213 break;
Artiom 0:bc74da1c502f 214
Artiom 0:bc74da1c502f 215 case 5:
Artiom 0:bc74da1c502f 216
Artiom 0:bc74da1c502f 217 break;
Artiom 0:bc74da1c502f 218
Artiom 0:bc74da1c502f 219 #endif
Artiom 0:bc74da1c502f 220 }
Artiom 0:bc74da1c502f 221 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 222
Artiom 0:bc74da1c502f 223 }
Artiom 0:bc74da1c502f 224 }