![](/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:
- 16:2a66cfe37262
- Parent:
- 15:119fc452dcaf
- Child:
- 17:b8ffd3593d0e
- Child:
- 18:0ea0d887966f
--- a/main.cpp Tue May 21 15:07:08 2019 +0000 +++ b/main.cpp Tue May 21 15:27:07 2019 +0000 @@ -438,11 +438,11 @@ 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: - SendRawId(HACHEUR_STATUT_VENTOUSES); + case attente_ack_ventouse: if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) { fgoldenium_avant=0; SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); @@ -470,11 +470,11 @@ 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: - SendRawId(HACHEUR_STATUT_VENTOUSES); + case attente_ack_ventouse: if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) { fgoldenium_arriere=0; SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); @@ -575,13 +575,13 @@ 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)); - SendRawId(HACHEUR_STATUT_VENTOUSES); if((status_pompe&MASK_AV_DROIT)== 0) { fbalance_avant=0; presentoir_arriere(); @@ -616,13 +616,13 @@ 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)); - SendRawId(HACHEUR_STATUT_VENTOUSES); if((status_pompe&MASK_AR_DROIT)== 0) { fbalance_arriere=0; presentoir_arriere(); @@ -650,12 +650,12 @@ 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); - SendRawId(HACHEUR_STATUT_VENTOUSES); if((status_pompe&MASK_PRESENTOIR_AV)== 0) { favant_relache=0; SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); @@ -680,22 +680,20 @@ 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); - SendRawId(HACHEUR_STATUT_VENTOUSES); + 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) { @@ -712,13 +710,13 @@ 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)); - SendRawId(HACHEUR_STATUT_VENTOUSES); if((status_pompe&MASK_SOL_AV)== 0) { sol_avant_remonter(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); @@ -747,13 +745,13 @@ 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)); - SendRawId(HACHEUR_STATUT_VENTOUSES); if((status_pompe&MASK_SOL_AR)== 0) { sol_arriere_remonter(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); @@ -768,7 +766,7 @@ } void automate_ventouse_accelerateur_avant (void) { - typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; + typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat; static type_etat etat = init; switch(etat) { @@ -781,15 +779,24 @@ SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); accelerateur_avant(); GoStraight(200,1,35,0);//35epaisseur accelerateur - etat = attente_ack_ventouse; + SendRawId(HACHEUR_STATUT_VENTOUSES); + etat = attente_ack_ventouse_droite_gauche; break; - case attente_ack_ventouse: + 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)); - SendRawId(HACHEUR_STATUT_VENTOUSES); + 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(); @@ -797,6 +804,7 @@ SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); etat = init; } + break; }