Carte esclave gros robot

Dependencies:   mbed Herkulex_Library_2019 ident_crac actions_Pr

Committer:
marwanesaich
Date:
Sat May 25 00:59:11 2019 +0000
Revision:
42:a7300d563596
Parent:
41:a71d06f292ac
Child:
43:7fff7f4d79a1

        

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