CRAC Team / Mbed 2 deprecated ACRAC_carte_esclave_GROS_ROBOT_2019

Carte esclave gros robot

Dependencies:   mbed Herkulex_Library_2019 ident_crac actions_Pr

main.cpp@30:d034f469359e, 2019-05-22 (annotated)

Committer:
Artiom
Date:
Wed May 22 21:49:04 2019 +0000
Revision:
30:d034f469359e
Parent:
25:9627700d989f
Child:
31:29500874c1cc
test pas de modif

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kyxstark 25:9627700d989f 1
Artiom 1:568955af8c2b 2 #include "main.h"
Artiom 0:bc74da1c502f 3
Artiom 0:bc74da1c502f 4 #define SIZE_FIFO 50
Artiom 0:bc74da1c502f 5
Artiom 30:d034f469359e 6
Artiom 0:bc74da1c502f 7 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom 0:bc74da1c502f 8
kyxstark 22:9e92a2b941ba 9 Serial pc(USBTX,USBRX);
Artiom 0:bc74da1c502f 10
Artiom 0:bc74da1c502f 11 CANMessage msgRxBuffer[SIZE_FIFO];
Artiom 0:bc74da1c502f 12 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom 0:bc74da1c502f 13 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom 0:bc74da1c502f 14 unsigned char EtatGameEnd=0;
Artiom 0:bc74da1c502f 15
kyxstark 21:edf6d03fdb20 16 void canProcessRx(void);
Artiom 12:191486ba5118 17
Artiom 12:191486ba5118 18
Artiom 0:bc74da1c502f 19 /*********************************************************************************************/
Artiom 0:bc74da1c502f 20 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 21 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 22 /*********************************************************************************************/
Artiom 0:bc74da1c502f 23 void canRx_ISR (void)
Artiom 0:bc74da1c502f 24 {
Artiom 0:bc74da1c502f 25 if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom 0:bc74da1c502f 26 if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom 0:bc74da1c502f 27
Artiom 0:bc74da1c502f 28 }
Artiom 0:bc74da1c502f 29 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 30
Artiom 0:bc74da1c502f 31 }
Artiom 0:bc74da1c502f 32
Artiom 0:bc74da1c502f 33 }
Artiom 0:bc74da1c502f 34
Artiom 0:bc74da1c502f 35 int main()
Artiom 0:bc74da1c502f 36 {
Artiom 0:bc74da1c502f 37 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 38 servo_interrupt_en(); //permettre les interuptions
Artiom 11:6c11c081c855 39
Artiom 0:bc74da1c502f 40 wait(1);//attente servo boot
kyxstark 21:edf6d03fdb20 41
kyxstark 21:edf6d03fdb20 42
kyxstark 21:edf6d03fdb20 43 #ifdef ROBOT_SMALL
kyxstark 22:9e92a2b941ba 44 pc.printf("\nPetit robot\n\n");
Artiom 2:9e63099cca99 45 gabarit_petit_robot();
kyxstark 21:edf6d03fdb20 46 #endif
kyxstark 21:edf6d03fdb20 47
kyxstark 21:edf6d03fdb20 48
kyxstark 21:edf6d03fdb20 49
kyxstark 21:edf6d03fdb20 50 #ifdef ROBOT_BIG
kyxstark 22:9e92a2b941ba 51 pc.printf("\nGros robot\n\n");
kyxstark 21:edf6d03fdb20 52 clear(doigt,2);
kyxstark 21:edf6d03fdb20 53 setTorque(doigt, TORQUE_ON,2);
kyxstark 21:edf6d03fdb20 54 positionControl(doigt,384,100,BLED_ON,2);
kyxstark 21:edf6d03fdb20 55 wait(1.0);
kyxstark 21:edf6d03fdb20 56 #endif
kyxstark 21:edf6d03fdb20 57
kyxstark 21:edf6d03fdb20 58
kyxstark 22:9e92a2b941ba 59 pc.printf("\nLAUNCHED");
Artiom 0:bc74da1c502f 60 while(1) {
Artiom 0:bc74da1c502f 61 canProcessRx();
Artiom 10:48c0d1b8aed9 62 f_mesure();//dt35
Artiom 11:6c11c081c855 63
kyxstark 21:edf6d03fdb20 64
kyxstark 21:edf6d03fdb20 65 #ifdef ROBOT_SMALL
Artiom 5:6e198cdd99ad 66 automate_ventouse_presentoir_arriere();
kyxstark 24:33c7ff467bd9 67 automate_ventouse_presentoir_avant();
Artiom 7:553f3f1c2c53 68
Artiom 6:45f9cf44718a 69 automate_ventouse_goldenium_arriere();
kyxstark 24:33c7ff467bd9 70 automate_ventouse_goldenium_avant();
kyxstark 24:33c7ff467bd9 71
Artiom 6:45f9cf44718a 72 automate_ventouse_sol_avant();
Artiom 7:553f3f1c2c53 73 automate_ventouse_sol_arriere();
Artiom 7:553f3f1c2c53 74
kyxstark 24:33c7ff467bd9 75 automate_ventouse_balance_avant();
Artiom 12:191486ba5118 76 automate_ventouse_balance_arriere();
Artiom 12:191486ba5118 77
Artiom 12:191486ba5118 78 automate_ventouse_relache_arriere();
kyxstark 24:33c7ff467bd9 79 automate_ventouse_relache_avant();
Artiom 14:b1abd2f6df75 80
Artiom 13:9e19048eb065 81 automate_ventouse_sol_avant_relache();
Artiom 14:b1abd2f6df75 82 automate_ventouse_sol_arriere_relache();
Artiom 14:b1abd2f6df75 83
Artiom 14:b1abd2f6df75 84 automate_ventouse_accelerateur_arriere();
kyxstark 24:33c7ff467bd9 85 automate_ventouse_accelerateur_avant();
kyxstark 21:edf6d03fdb20 86 #endif
kyxstark 21:edf6d03fdb20 87
kyxstark 21:edf6d03fdb20 88 #ifdef ROBOT_BIG
kyxstark 25:9627700d989f 89 /*automate_ventouse_presentoir_avant();
kyxstark 24:33c7ff467bd9 90 automate_ventouse_goldenium_avant();
kyxstark 24:33c7ff467bd9 91 automate_ventouse_relache_avant();
kyxstark 25:9627700d989f 92 automate_ventouse_accelerateur_avant();*/
kyxstark 24:33c7ff467bd9 93
kyxstark 21:edf6d03fdb20 94 fifo_couleur();
kyxstark 21:edf6d03fdb20 95 ascenseur();
kyxstark 21:edf6d03fdb20 96
kyxstark 21:edf6d03fdb20 97 #endif
Artiom 13:9e19048eb065 98
Artiom 0:bc74da1c502f 99 if(EtatGameEnd==1) {
Artiom 0:bc74da1c502f 100 while(1);
Artiom 0:bc74da1c502f 101 }
Artiom 0:bc74da1c502f 102
Artiom 0:bc74da1c502f 103 }
Artiom 0:bc74da1c502f 104 }
Artiom 0:bc74da1c502f 105
Artiom 0:bc74da1c502f 106 /****************************************************************************************/
Artiom 0:bc74da1c502f 107 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 108 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 109 /****************************************************************************************/
Artiom 0:bc74da1c502f 110 void canProcessRx(void)
Artiom 0:bc74da1c502f 111 {
Artiom 0:bc74da1c502f 112 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 113 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 114 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 115 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 116 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 0:bc74da1c502f 117 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 118 FIFO_max_occupation=FIFO_occupation;
Artiom 0:bc74da1c502f 119 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 120 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 121
Artiom 0:bc74da1c502f 122 switch(identifiant) {
Artiom 0:bc74da1c502f 123
Artiom 0:bc74da1c502f 124 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 125 EtatGameEnd = 1;
Artiom 0:bc74da1c502f 126 break;
Artiom 0:bc74da1c502f 127
Artiom 0:bc74da1c502f 128 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 129 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 130 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 131
Artiom 0:bc74da1c502f 132
Artiom 0:bc74da1c502f 133 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 134 msgTx.len=2;
Artiom 0:bc74da1c502f 135 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 136 msgTx.type=CANData;
Artiom 0:bc74da1c502f 137 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 138 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 139 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 140
Artiom 0:bc74da1c502f 141 can.write(msgTx);
Artiom 0:bc74da1c502f 142 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 143 break;
Artiom 0:bc74da1c502f 144
Artiom 0:bc74da1c502f 145 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 146 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 147 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 148 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 149 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 150
Artiom 0:bc74da1c502f 151 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 152 msgTx.len=8;
Artiom 0:bc74da1c502f 153 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 154 msgTx.type=CANData;
Artiom 0:bc74da1c502f 155 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 156 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 157 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 158 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 159 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 160 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 161 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 162 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 163 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 164 can.write(msgTx);
Artiom 0:bc74da1c502f 165 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 166 break;
kyxstark 23:93a427b242ce 167
kyxstark 25:9627700d989f 168
kyxstark 25:9627700d989f 169
kyxstark 25:9627700d989f 170 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
kyxstark 25:9627700d989f 171
kyxstark 25:9627700d989f 172
kyxstark 25:9627700d989f 173 case HACHEUR_STATUT_VENTOUSES:
kyxstark 25:9627700d989f 174 status_pompe = msgRxBuffer[FIFO_lecture].data[1];
kyxstark 25:9627700d989f 175 //can.write(CANMessage(0x529, &status_pompe,1));
kyxstark 25:9627700d989f 176 break;
kyxstark 25:9627700d989f 177
kyxstark 25:9627700d989f 178 case HACHEUR_GET_ATOM_ACK:
kyxstark 25:9627700d989f 179 status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
kyxstark 25:9627700d989f 180 //can.write(CANMessage(0x529, &status_pompe,1));
kyxstark 25:9627700d989f 181 break;
kyxstark 25:9627700d989f 182
kyxstark 25:9627700d989f 183 case HACHEUR_RELEASE_ATOM_ACK :
kyxstark 25:9627700d989f 184 status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
kyxstark 25:9627700d989f 185 break;
kyxstark 25:9627700d989f 186
kyxstark 25:9627700d989f 187 //-------------------------------------------------------------------------------------------------------------------------------------------
kyxstark 25:9627700d989f 188
kyxstark 25:9627700d989f 189
kyxstark 25:9627700d989f 190
kyxstark 25:9627700d989f 191 #ifdef ROBOT_SMALL
kyxstark 23:93a427b242ce 192 case PRESENTOIR_AVANT:
kyxstark 23:93a427b242ce 193 fpresentoir_avant=1;
kyxstark 23:93a427b242ce 194 break;
kyxstark 23:93a427b242ce 195
kyxstark 23:93a427b242ce 196 case BALANCE_AVANT:
kyxstark 23:93a427b242ce 197 fbalance_avant=1;
kyxstark 23:93a427b242ce 198 break;
kyxstark 23:93a427b242ce 199
kyxstark 23:93a427b242ce 200 case ACCELERATEUR_AVANT:
kyxstark 23:93a427b242ce 201 faccelerateur_avant=1;
kyxstark 23:93a427b242ce 202 break;
kyxstark 23:93a427b242ce 203
kyxstark 23:93a427b242ce 204 case GOLDENIUM_AVANT:
kyxstark 23:93a427b242ce 205 fgoldenium_avant=1;
kyxstark 23:93a427b242ce 206 break;
kyxstark 23:93a427b242ce 207
kyxstark 23:93a427b242ce 208 case VENTOUSE_AV_CENTRE_BALANCE:
kyxstark 23:93a427b242ce 209 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 23:93a427b242ce 210 goldenium_avant();
kyxstark 23:93a427b242ce 211 verification();
kyxstark 23:93a427b242ce 212 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 23:93a427b242ce 213 break;
kyxstark 23:93a427b242ce 214
kyxstark 23:93a427b242ce 215 case AVANT_RELACHE:
kyxstark 23:93a427b242ce 216 favant_relache=1;
kyxstark 23:93a427b242ce 217 break;
Artiom 4:4a79942713fa 218 //-------------------------------------------------------------------------Actions petit robot----------------------------------------------
Artiom 2:9e63099cca99 219 case GABARIT_PETIT_ROBOT:
Artiom 2:9e63099cca99 220 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 2:9e63099cca99 221 gabarit_petit_robot();
Artiom 2:9e63099cca99 222 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:bc74da1c502f 223 break;
Artiom 0:bc74da1c502f 224
Artiom 0:bc74da1c502f 225 case PRESENTOIR_ARRIERE:
Artiom 5:6e198cdd99ad 226 fpresentoir_arriere=1;
Artiom 0:bc74da1c502f 227 break;
Artiom 0:bc74da1c502f 228
Artiom 0:bc74da1c502f 229 case BALANCE_ARRIERE:
Artiom 12:191486ba5118 230 fbalance_arriere=1;
Artiom 2:9e63099cca99 231 break;
Artiom 2:9e63099cca99 232
Artiom 2:9e63099cca99 233 case ACCELERATEUR_ARRIERE:
Artiom 14:b1abd2f6df75 234 faccelerateur_arriere=1;
Artiom 2:9e63099cca99 235 break;
Artiom 6:45f9cf44718a 236
Artiom 2:9e63099cca99 237 case GOLDENIUM_ARRIERE:
Artiom 6:45f9cf44718a 238 fgoldenium_arriere=1;
Artiom 2:9e63099cca99 239 break;
Artiom 6:45f9cf44718a 240
Artiom 2:9e63099cca99 241 case SOL_AVANT:
Artiom 6:45f9cf44718a 242 fsol_avant=1;
Artiom 2:9e63099cca99 243 break;
Artiom 2:9e63099cca99 244
Artiom 2:9e63099cca99 245 case SOL_ARRIERE:
Artiom 6:45f9cf44718a 246 fsol_arriere=1;
Artiom 2:9e63099cca99 247 break;
Artiom 2:9e63099cca99 248
Artiom 2:9e63099cca99 249 case SOL_AVANT_RELACHE:
Artiom 7:553f3f1c2c53 250 fsol_avant_relache=1;
Artiom 2:9e63099cca99 251 break;
Artiom 2:9e63099cca99 252
Artiom 2:9e63099cca99 253 case SOL_ARRIERE_RELACHE:
Artiom 7:553f3f1c2c53 254 fsol_arriere_relache=1;
Artiom 0:bc74da1c502f 255 break;
Artiom 12:191486ba5118 256
Artiom 12:191486ba5118 257 case ARRIERE_RELACHE:
Artiom 12:191486ba5118 258 farriere_relache=1;
Artiom 12:191486ba5118 259 break;
Artiom 14:b1abd2f6df75 260 case RECROQUEVILLER:
Artiom 14:b1abd2f6df75 261 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:b1abd2f6df75 262 recroqueviller_avant();
Artiom 14:b1abd2f6df75 263 recroqueviller_arriere();
Artiom 14:b1abd2f6df75 264 verification();
Artiom 14:b1abd2f6df75 265 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 266 break;
Artiom 13:9e19048eb065 267
Artiom 20:42f8ec726ac8 268 case VENTOUSE_AR_CENTRE_BALANCE:
Artiom 20:42f8ec726ac8 269 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 20:42f8ec726ac8 270 goldenium_arriere();
Artiom 20:42f8ec726ac8 271 positionControl(AR_poigne_D, 200,1,BLED_ON,3);//actionneur
Artiom 20:42f8ec726ac8 272 verification();
Artiom 20:42f8ec726ac8 273 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 274 break;
Artiom 13:9e19048eb065 275
Artiom 0:bc74da1c502f 276
Artiom 0:bc74da1c502f 277 #endif
kyxstark 23:93a427b242ce 278
Artiom 0:bc74da1c502f 279 #ifdef ROBOT_BIG
kyxstark 21:edf6d03fdb20 280 case ASCENSEUR:
kyxstark 22:9e92a2b941ba 281 flag_ascenseur = msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 282 break;
Artiom 0:bc74da1c502f 283
Artiom 0:bc74da1c502f 284 #endif
Artiom 0:bc74da1c502f 285 }
Artiom 0:bc74da1c502f 286 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 287
Artiom 0:bc74da1c502f 288 }
Artiom 4:4a79942713fa 289
Artiom 4:4a79942713fa 290
Artiom 4:4a79942713fa 291 }
Artiom 4:4a79942713fa 292
Artiom 4:4a79942713fa 293
Artiom 4:4a79942713fa 294
Artiom 11:6c11c081c855 295