CRAC Team
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)
-{
-
-}
-
-