![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
carte esclave Petit Robot
Dependencies: mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr
Diff: main.cpp
- Revision:
- 21:edf6d03fdb20
- Parent:
- 20:42f8ec726ac8
- Child:
- 22:9e92a2b941ba
--- a/main.cpp Wed May 22 08:25:51 2019 +0000 +++ b/main.cpp Wed May 22 11:19:27 2019 +0000 @@ -1,10 +1,5 @@ #include "mbed.h" -#include "ident_crac.h" -#include "Capteur.h" -#include "Actionneurs.h" -#include "fonctions_herkulex.h" #include "main.h" -#include "dt.h" #define SIZE_FIFO 50 @@ -16,45 +11,17 @@ signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN unsigned char EtatGameEnd=0; -char fpresentoir_avant=0, fpresentoir_arriere=0; -char fgoldenium_avant=0, fgoldenium_arriere=0; -char fsol_avant=0,fsol_arriere=0; -char fsol_avant_relache=0,fsol_arriere_relache=0; -char fbalance_avant=0,fbalance_arriere=0; -char favant_relache=0,farriere_relache=0; -char faccelerateur_avant=0,faccelerateur_arriere=0;; + +int rotation_couroies = 0; + + +void canProcessRx(void); -char status_pompe=0; -void canRx_ISR (void); -void SendAck(unsigned short id, unsigned short from); -void SendRawId (unsigned short id); -void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement); -void canProcessRx(void); - -void automate_ventouse_presentoir_avant(void); -void automate_ventouse_presentoir_arriere (void); - -void automate_ventouse_goldenium_avant (void); -void automate_ventouse_goldenium_arriere (void); - -void automate_ventouse_sol_avant (void); -void automate_ventouse_sol_arriere(void); - -void automate_ventouse_sol_avant_relache(void); -void automate_ventouse_sol_arriere_relache(void); - -void automate_ventouse_balance_avant (void); -void automate_ventouse_balance_arriere(void); - -void automate_ventouse_relache_avant(void); -void automate_ventouse_relache_arriere (void); - -void automate_ventouse_accelerateur_avant(void); -void automate_ventouse_accelerateur_arriere(void); - +#ifdef ROBOT_BIG +#endif /*********************************************************************************************/ /* FUNCTION NAME: canRx_ISR */ @@ -71,85 +38,6 @@ } } -/*********************************************************************************************/ -/* FUNCTION NAME: SendAck */ -/* DESCRIPTION : */ -/*********************************************************************************************/ -void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale -{ - CANMessage msgTx=CANMessage(); - msgTx.id=from; //waitingAckFrom - msgTx.len=2; - msgTx.format=CANStandard; - msgTx.type=CANData; - // from sur 2 octets - msgTx.data[0]=(unsigned char)id; //waitingAckID - msgTx.data[1]=(unsigned char)(id>>8); - can.write(msgTx); -} -/*********************************************************************************************/ -/* FUNCTION NAME: SendRawId */ -/* DESCRIPTION : */ -/*********************************************************************************************/ -void SendRawId (unsigned short id) -{ - CANMessage msgTx=CANMessage(); - msgTx.id=id; - msgTx.len=0; - can.write(msgTx); //carte esclave f446re - wait_us(200); -} - -/*********************************************************************************************/ -/* FUNCTION NAME: GoStraight */ -/* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */ -/* recalage : 0 => pas de recalage */ -/* 1 => recalage en X */ -/* 2 => Recalage en Y */ -/* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */ -/* isEnchainement : Indique si il faut executer l'instruction en enchainement */ -/* 0 => non */ -/* 1 => oui */ -/* 2 => dernière instruction de l'enchainement */ -/*********************************************************************************************/ -void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement) -{ - CANMessage msgTx=CANMessage(); - msgTx.id=ASSERVISSEMENT_RECALAGE; - msgTx.len=6; - msgTx.format=CANStandard; - msgTx.type=CANData; - // x sur 2 octets - msgTx.data[0]=(unsigned char)distance; - msgTx.data[1]=(unsigned char)(distance>>8); - //Recalage sur 1 octet - msgTx.data[2]=recalage; - //Valeur du recalage sur 2 octets - msgTx.data[3]=(unsigned char)newValue; - msgTx.data[4]=(unsigned char)(newValue>>8); - //Enchainement sur 1 octet - msgTx.data[5]=isEnchainement; - - can.write(msgTx); - //wait_ms(500); -} -/****************************************************************************************/ -/* FUNCTION NAME: Rotate */ -/* DESCRIPTION : Transmission CAN correspondant à une rotation */ -/****************************************************************************************/ -void Rotate (signed short angle) -{ - CANMessage msgTx=CANMessage(); - msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot - msgTx.len=2; - msgTx.format=CANStandard; - msgTx.type=CANData; - // Angle signé sur 2 octets - msgTx.data[0]=(unsigned char)angle; - msgTx.data[1]=(unsigned char)(angle>>8); - - can.write(msgTx); -} int main() { @@ -157,11 +45,28 @@ servo_interrupt_en(); //permettre les interuptions wait(1);//attente servo boot + + +#ifdef ROBOT_SMALL gabarit_petit_robot(); +#endif + + + +#ifdef ROBOT_BIG + clear(doigt,2); + setTorque(doigt, TORQUE_ON,2); + positionControl(doigt,384,100,BLED_ON,2); + wait(1.0); +#endif + + while(1) { canProcessRx(); f_mesure();//dt35 + +#ifdef ROBOT_SMALL automate_ventouse_presentoir_avant(); automate_ventouse_presentoir_arriere(); @@ -182,6 +87,13 @@ automate_ventouse_accelerateur_avant(); automate_ventouse_accelerateur_arriere(); +#endif + +#ifdef ROBOT_BIG + fifo_couleur(); + ascenseur(); + +#endif if(EtatGameEnd==1) { while(1); @@ -361,7 +273,7 @@ #endif #ifdef ROBOT_BIG - case COUROIES: + case ASCENSEUR: rotation_couroies = msgRxBuffer[FIFO_lecture].data[0]; break; @@ -378,463 +290,6 @@ } -void automate_ventouse_presentoir_avant (void) -{ - - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fpresentoir_avant) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - presentoir_avant(); - can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AV)); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) { - fpresentoir_avant=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } -} -void automate_ventouse_presentoir_arriere (void) -{ - - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fpresentoir_arriere) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - presentoir_arriere(); - can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AR)); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) { - fpresentoir_arriere=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } -} -void automate_ventouse_goldenium_avant (void) -{ - - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fgoldenium_avant) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - goldenium_avant(); - char pompe=AV_CENTRE; - can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1)); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) { - fgoldenium_avant=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } -} - -void automate_ventouse_goldenium_arriere (void) -{ - - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fgoldenium_arriere) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - goldenium_arriere(); - char pompe=AR_CENTRE; - can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1)); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) { - fgoldenium_arriere=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } - - -} - -void automate_ventouse_sol_avant (void) -{ - - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fsol_avant) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - sol_avant_baisser(); - char pompe=AV_BAS; - can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1)); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) { - sol_avant_remonter(); - fsol_avant=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } - - -} -void automate_ventouse_sol_arriere (void) -{ - - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fsol_arriere) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - sol_arriere_baisser(); - char pompe=AR_BAS; - can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1)); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) { - sol_arriere_remonter(); - fsol_arriere=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } -} -void automate_ventouse_balance_avant (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - switch(etat) { - case init: //attente d'initialisation - if(fbalance_avant) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - presentoir_avant(); - verification(); - balance_avant_placement(); - Rotate(200); - wait(0.5); - balance_avant(); - Rotate(-200); - wait(0.5); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - char pompe=AV_DROIT; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - if((status_pompe&MASK_AV_DROIT)== 0) { - fbalance_avant=0; - presentoir_arriere(); - presentoir_avant(); - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant(); - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } - - -} -void automate_ventouse_balance_arriere (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fbalance_arriere) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - presentoir_arriere(); - verification(); - balance_arriere_placement(); - Rotate(200); - wait(0.5); - balance_arriere(); - Rotate(-200); - wait(0.5); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - char pompe=AR_DROIT; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - if((status_pompe&MASK_AR_DROIT)== 0) { - fbalance_arriere=0; - presentoir_arriere(); - presentoir_avant(); - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant(); - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } - - -} -void automate_ventouse_relache_avant (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(favant_relache) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - SendRawId(HACHEUR_RELEASE_AV); - if((status_pompe&MASK_PRESENTOIR_AV)== 0) { - favant_relache=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - - } - - -} -void automate_ventouse_relache_arriere (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(farriere_relache) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - SendRawId(HACHEUR_RELEASE_AR); - if((status_pompe&MASK_PRESENTOIR_AR)== 0) { - farriere_relache=0; - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - break; - } - -} -void automate_ventouse_sol_avant_relache (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fsol_avant_relache) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - positionControl(AV_sol,520,1,BLED_ON,4);//baisser - verification(); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - char pompe=AV_BAS; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - if((status_pompe&MASK_SOL_AV)== 0) { - sol_avant_remonter(); - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - fsol_avant_relache=0; - etat = init; - } - break; - - } - - -} - -void automate_ventouse_sol_arriere_relache (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(fsol_arriere_relache) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - positionControl(AR_sol,480,1,BLED_ON,4);//baisser - verification(); - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse; - break; - - case attente_ack_ventouse: - char pompe=AR_BAS; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - if((status_pompe&MASK_SOL_AR)== 0) { - sol_arriere_remonter(); - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - fsol_arriere_relache=0; - etat = init; - } - break; - - } - - -} -void automate_ventouse_accelerateur_avant (void) -{ - typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat; - static type_etat etat = init; - - switch(etat) { - case init: //attente d'initialisation - if(faccelerateur_avant) - etat=envoi_instruction; - break; - - case envoi_instruction://envoi instruction - SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); - accelerateur_avant(); - GoStraight(200,1,35,0);//35epaisseur accelerateur - SendRawId(HACHEUR_STATUT_VENTOUSES); - etat = attente_ack_ventouse_droite_gauche; - break; - - case attente_ack_ventouse_droite_gauche: - char pompe=AV_DROIT; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - pompe=AV_GAUCHE; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) { - GoStraight(-20,0,0,0); - wait(1); - presentoir_avant(); - etat = attente_ack_ventouse_centre; - } - case attente_ack_ventouse_centre: - pompe=AV_CENTRE; - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1)); - if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) { - faccelerateur_avant=0; - presentoir_avant(); - can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant(); - SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); - etat = init; - } - - break; - - } - - -} - - -void automate_ventouse_accelerateur_arriere (void) -{ - -} - -