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@1:568955af8c2b, 2019-05-07 (annotated)

Committer:
Artiom
Date:
Tue May 07 17:27:38 2019 +0000
Revision:
1:568955af8c2b
Parent:
0:bc74da1c502f
Child:
2:9e63099cca99
Ajout de l'envoi de commande vers la carte pompe

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