![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |