CRAC Team
code ax12 petit robot 12/05/2017
Fork of
command_AX12_petit_robot_V3
by 
CRAC Team
main.cpp@4:888b32bc5b63, 2017-05-02 (annotated)
- Committer:
- SquirrelGod
- Date:
- Tue May 02 12:06:47 2017 +0000
- Revision:
- 4:888b32bc5b63
- Parent:
- 3:6cbd7d2c78f4
- Child:
- 6:c239c7abd7fe
pour clement 02/05/2017
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| mathieulegros | 0:a2a44c043932 | 1 | #include "mbed.h" |
| mathieulegros | 0:a2a44c043932 | 2 | #include "AX12.h" |
| mathieulegros | 0:a2a44c043932 | 3 | #include "cmsis.h" |
| SquirrelGod | 2:99b1cb0d9f5e | 4 | #include "ident_crac.h" |
| mathieulegros | 0:a2a44c043932 | 5 | |
| SquirrelGod | 4:888b32bc5b63 | 6 | #define AX12_INITIALISATION 0 |
| SquirrelGod | 4:888b32bc5b63 | 7 | #define AX12_PREPARATION_PRISE 1 |
| SquirrelGod | 4:888b32bc5b63 | 8 | #define AX12_STOCKAGE_HAUT 2 |
| SquirrelGod | 4:888b32bc5b63 | 9 | #define AX12_STOCKAGE_BAS 3 |
| SquirrelGod | 4:888b32bc5b63 | 10 | #define AX12_DEPOSER 4 |
| SquirrelGod | 4:888b32bc5b63 | 11 | #define AX12_PREPARATION_DEPOT_BAS 5 |
| SquirrelGod | 4:888b32bc5b63 | 12 | #define AX12_PREPARATION_DEPOT_HAUT 6 |
| SquirrelGod | 4:888b32bc5b63 | 13 | #define AX12_POUSSER_MODULE 7 |
| SquirrelGod | 4:888b32bc5b63 | 14 | #define AX12_DEFAUT 20 |
| SquirrelGod | 4:888b32bc5b63 | 15 | |
| SquirrelGod | 2:99b1cb0d9f5e | 16 | #define TIME 0.8 |
| SquirrelGod | 4:888b32bc5b63 | 17 | #define SIZE_FIFO 25 |
| SquirrelGod | 2:99b1cb0d9f5e | 18 | |
| mathieulegros | 0:a2a44c043932 | 19 | /* DECLARATION VARIABLES */ |
| SquirrelGod | 2:99b1cb0d9f5e | 20 | CAN can1(p30,p29); |
| SquirrelGod | 2:99b1cb0d9f5e | 21 | CANMessage msgRxBuffer[SIZE_FIFO]; |
| SquirrelGod | 2:99b1cb0d9f5e | 22 | unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN |
| SquirrelGod | 2:99b1cb0d9f5e | 23 | unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN |
| SquirrelGod | 2:99b1cb0d9f5e | 24 | |
| SquirrelGod | 2:99b1cb0d9f5e | 25 | extern "C" void mbed_reset();//Pour pouvoir reset la carte |
| SquirrelGod | 2:99b1cb0d9f5e | 26 | |
| SquirrelGod | 4:888b32bc5b63 | 27 | unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0, action_precedente = 0; |
| SquirrelGod | 2:99b1cb0d9f5e | 28 | short vitesse=700; |
| SquirrelGod | 2:99b1cb0d9f5e | 29 | float angle=0.0; |
| SquirrelGod | 2:99b1cb0d9f5e | 30 | float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0; |
| SquirrelGod | 4:888b32bc5b63 | 31 | AX12 *un_myAX12, *sept_myAX12, *huit_myAX12, *seize_myAX12, *multiple_myAX12; //Pince centrale |
| SquirrelGod | 4:888b32bc5b63 | 32 | AX12 *un_myAX12_2, *deux_myAX12_2, *trois_myAX12_2, *multiple_myAX12_2; //Bras de gauche |
| SquirrelGod | 2:99b1cb0d9f5e | 33 | |
| SquirrelGod | 2:99b1cb0d9f5e | 34 | |
| SquirrelGod | 2:99b1cb0d9f5e | 35 | /* PROTOTYPES DE FONCTIONS ET POINTEURS */ |
| SquirrelGod | 2:99b1cb0d9f5e | 36 | |
| SquirrelGod | 4:888b32bc5b63 | 37 | |
| SquirrelGod | 4:888b32bc5b63 | 38 | |
| SquirrelGod | 2:99b1cb0d9f5e | 39 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 40 | /* FUNCTION NAME: canProcessRx */ |
| SquirrelGod | 2:99b1cb0d9f5e | 41 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
| SquirrelGod | 2:99b1cb0d9f5e | 42 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 43 | void canProcessRx(void); |
| SquirrelGod | 2:99b1cb0d9f5e | 44 | |
| SquirrelGod | 2:99b1cb0d9f5e | 45 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 46 | /* FUNCTION NAME: canRx_ISR */ |
| SquirrelGod | 2:99b1cb0d9f5e | 47 | /* DESCRIPTION : Interruption en réception sur le CAN */ |
| SquirrelGod | 2:99b1cb0d9f5e | 48 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 49 | void canRx_ISR (void); |
| SquirrelGod | 2:99b1cb0d9f5e | 50 | |
| SquirrelGod | 2:99b1cb0d9f5e | 51 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 52 | /* FUNCTION NAME: SendRawId */ |
| SquirrelGod | 2:99b1cb0d9f5e | 53 | /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ |
| SquirrelGod | 2:99b1cb0d9f5e | 54 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 55 | void SendRawId (unsigned short id); |
| SquirrelGod | 2:99b1cb0d9f5e | 56 | |
| SquirrelGod | 2:99b1cb0d9f5e | 57 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 58 | /* FUNCTION NAME: Fin_action */ |
| SquirrelGod | 4:888b32bc5b63 | 59 | /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ |
| SquirrelGod | 4:888b32bc5b63 | 60 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 61 | void Fin_action(void); |
| SquirrelGod | 4:888b32bc5b63 | 62 | |
| SquirrelGod | 4:888b32bc5b63 | 63 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 64 | /* FUNCTION NAME: Automate_ax12 */ |
| SquirrelGod | 4:888b32bc5b63 | 65 | /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ |
| SquirrelGod | 4:888b32bc5b63 | 66 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 67 | void AX12_automate(void); |
| SquirrelGod | 4:888b32bc5b63 | 68 | |
| SquirrelGod | 4:888b32bc5b63 | 69 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 70 | /* FUNCTION NAME: Initialisation_position */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 71 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 72 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 73 | void Initialisation_position(void); |
| SquirrelGod | 3:6cbd7d2c78f4 | 74 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 75 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 76 | /* FUNCTION NAME: Preparation_prise */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 77 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 78 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 79 | void Preparation_prise(void); |
| SquirrelGod | 3:6cbd7d2c78f4 | 80 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 81 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 82 | /* FUNCTION NAME: Stockage_haut */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 83 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 84 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 85 | void Stockage_haut(void); |
| SquirrelGod | 2:99b1cb0d9f5e | 86 | |
| SquirrelGod | 2:99b1cb0d9f5e | 87 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 88 | /* FUNCTION NAME: Stockage_bas */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 89 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
| SquirrelGod | 2:99b1cb0d9f5e | 90 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 91 | void Stockage_bas(void); |
| SquirrelGod | 3:6cbd7d2c78f4 | 92 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 93 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 94 | /* FUNCTION NAME: Deposer */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 95 | /* DESCRIPTION : Fonction qui permet de déposer un module */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 96 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 97 | void Deposer(void); |
| SquirrelGod | 2:99b1cb0d9f5e | 98 | |
| SquirrelGod | 2:99b1cb0d9f5e | 99 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 100 | /* FUNCTION NAME: Preparation_depos_bas */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 101 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
| SquirrelGod | 2:99b1cb0d9f5e | 102 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 103 | void Preparation_depot_bas(void); |
| SquirrelGod | 2:99b1cb0d9f5e | 104 | |
| SquirrelGod | 2:99b1cb0d9f5e | 105 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 106 | /* FUNCTION NAME: Preparation_depos_haut */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 107 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
| SquirrelGod | 2:99b1cb0d9f5e | 108 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 109 | void Preparation_depot_haut(void); |
| SquirrelGod | 3:6cbd7d2c78f4 | 110 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 111 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 112 | /* FUNCTION NAME: Pousser_module */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 113 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 114 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 115 | void Pousser_module(void); |
| SquirrelGod | 4:888b32bc5b63 | 116 | |
| SquirrelGod | 4:888b32bc5b63 | 117 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 118 | /* FUNCTION NAME: Initialisation_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 119 | /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ |
| SquirrelGod | 4:888b32bc5b63 | 120 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 121 | void Initialisation_gauche(void); |
| SquirrelGod | 4:888b32bc5b63 | 122 | |
| SquirrelGod | 4:888b32bc5b63 | 123 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 124 | /* FUNCTION NAME: Preparation_prise_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 125 | /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 126 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 127 | void Preparation_prise_gauche(void); |
| SquirrelGod | 4:888b32bc5b63 | 128 | |
| SquirrelGod | 4:888b32bc5b63 | 129 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 130 | /* FUNCTION NAME: Prendre_module_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 131 | /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 132 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 133 | void Prendre_module_gauche(void); |
| SquirrelGod | 4:888b32bc5b63 | 134 | |
| SquirrelGod | 4:888b32bc5b63 | 135 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 136 | /* FUNCTION NAME: Tourner_module_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 137 | /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 138 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 139 | void Tourner_module_gauche(void); |
| SquirrelGod | 4:888b32bc5b63 | 140 | |
| SquirrelGod | 4:888b32bc5b63 | 141 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 142 | /* FUNCTION NAME: Preparatio_module_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 143 | /* DESCRIPTION : Fonction qui prepare le tournante */ |
| SquirrelGod | 4:888b32bc5b63 | 144 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 145 | void Preparation_module_gauche(void); |
| SquirrelGod | 2:99b1cb0d9f5e | 146 | |
| mathieulegros | 0:a2a44c043932 | 147 | Timer t; |
| mathieulegros | 0:a2a44c043932 | 148 | Ticker flipper; |
| SquirrelGod | 2:99b1cb0d9f5e | 149 | |
| SquirrelGod | 4:888b32bc5b63 | 150 | ////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// |
| SquirrelGod | 4:888b32bc5b63 | 151 | static char TAB1[25]= {0x01,0x05, 0x02, 0xFF, 0x00, ///Position initiale |
| SquirrelGod | 4:888b32bc5b63 | 152 | 0x10,0x00, 0x02, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 153 | 0x07,0x00, 0x01, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 154 | 0x08,0x00, 0x03, 0xFF, 0x00}; |
| ConstantMBED | 1:b3ff77670606 | 155 | |
| SquirrelGod | 4:888b32bc5b63 | 156 | static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise |
| SquirrelGod | 4:888b32bc5b63 | 157 | 0x10,0x50, 0x01, 0xFF, 0x03, |
| SquirrelGod | 3:6cbd7d2c78f4 | 158 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 159 | 0x08,0xF4, 0x01, 0xFF, 0x03}; |
| SquirrelGod | 3:6cbd7d2c78f4 | 160 | |
| SquirrelGod | 4:888b32bc5b63 | 161 | static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee) |
| SquirrelGod | 4:888b32bc5b63 | 162 | 0x10,0x50, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 163 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 164 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
| SquirrelGod | 4:888b32bc5b63 | 165 | |
| SquirrelGod | 4:888b32bc5b63 | 166 | static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air) |
| SquirrelGod | 4:888b32bc5b63 | 167 | 0x10,0x50, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 168 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 169 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
| SquirrelGod | 3:6cbd7d2c78f4 | 170 | |
| SquirrelGod | 4:888b32bc5b63 | 171 | static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) |
| SquirrelGod | 4:888b32bc5b63 | 172 | 0x10,0xF4, 0x01, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 173 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 174 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
| SquirrelGod | 2:99b1cb0d9f5e | 175 | |
| SquirrelGod | 4:888b32bc5b63 | 176 | static char TAB6[25]= {0x01,0xB0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
| SquirrelGod | 4:888b32bc5b63 | 177 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 178 | 0x07,0x00, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 179 | 0x08,0x00, 0x03, 0xFF, 0x03}; |
| SquirrelGod | 3:6cbd7d2c78f4 | 180 | |
| SquirrelGod | 4:888b32bc5b63 | 181 | static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air) |
| SquirrelGod | 4:888b32bc5b63 | 182 | 0x10,0xB0, 0x00, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 183 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 184 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
| ConstantMBED | 1:b3ff77670606 | 185 | |
| SquirrelGod | 4:888b32bc5b63 | 186 | static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas |
| SquirrelGod | 4:888b32bc5b63 | 187 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 188 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 189 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
| ConstantMBED | 1:b3ff77670606 | 190 | |
| SquirrelGod | 4:888b32bc5b63 | 191 | static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer |
| SquirrelGod | 4:888b32bc5b63 | 192 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 193 | 0x07,0xD0, 0x00, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 194 | 0x08,0x35, 0x03, 0xFF, 0x00}; |
| SquirrelGod | 3:6cbd7d2c78f4 | 195 | |
| SquirrelGod | 4:888b32bc5b63 | 196 | static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) |
| SquirrelGod | 4:888b32bc5b63 | 197 | 0x10,0x00, 0x01, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 198 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 199 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
| SquirrelGod | 3:6cbd7d2c78f4 | 200 | |
| SquirrelGod | 4:888b32bc5b63 | 201 | static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module |
| SquirrelGod | 4:888b32bc5b63 | 202 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 203 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 204 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
| SquirrelGod | 4:888b32bc5b63 | 205 | |
| SquirrelGod | 4:888b32bc5b63 | 206 | static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale |
| SquirrelGod | 4:888b32bc5b63 | 207 | 0x10,0x00, 0x02, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 208 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 209 | 0x08,0xF4, 0x01, 0xFF, 0x03}; |
| SquirrelGod | 3:6cbd7d2c78f4 | 210 | |
| SquirrelGod | 4:888b32bc5b63 | 211 | static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer |
| SquirrelGod | 4:888b32bc5b63 | 212 | 0x10,0xA0, 0x02, 0xFF, 0x03, |
| SquirrelGod | 4:888b32bc5b63 | 213 | 0x07,0xD0, 0x00, 0xFF, 0x00, |
| SquirrelGod | 4:888b32bc5b63 | 214 | 0x08,0x35, 0x03, 0xFF, 0x00}; |
| SquirrelGod | 4:888b32bc5b63 | 215 | |
| SquirrelGod | 4:888b32bc5b63 | 216 | ////////////////////// TABLEAU BRAS GAUCHE /////////////////////////// |
| SquirrelGod | 4:888b32bc5b63 | 217 | static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale |
| SquirrelGod | 4:888b32bc5b63 | 218 | 0x02,0xF4, 0x01, 0xFF, 0x03}; |
| SquirrelGod | 2:99b1cb0d9f5e | 219 | |
| SquirrelGod | 4:888b32bc5b63 | 220 | static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner |
| SquirrelGod | 4:888b32bc5b63 | 221 | 0x02,0x40, 0x03, 0xFF, 0x03}; |
| SquirrelGod | 4:888b32bc5b63 | 222 | |
| SquirrelGod | 4:888b32bc5b63 | 223 | static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module |
| SquirrelGod | 4:888b32bc5b63 | 224 | 0x02,0xE5, 0x02, 0xFF, 0x03}; |
| SquirrelGod | 4:888b32bc5b63 | 225 | |
| SquirrelGod | 4:888b32bc5b63 | 226 | |
| SquirrelGod | 4:888b32bc5b63 | 227 | |
| SquirrelGod | 2:99b1cb0d9f5e | 228 | |
| mathieulegros | 0:a2a44c043932 | 229 | /* ANGLE */ |
| mathieulegros | 0:a2a44c043932 | 230 | |
| mathieulegros | 0:a2a44c043932 | 231 | /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 |
| mathieulegros | 0:a2a44c043932 | 232 | 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 |
| mathieulegros | 0:a2a44c043932 | 233 | 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 |
| mathieulegros | 0:a2a44c043932 | 234 | 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 |
| mathieulegros | 0:a2a44c043932 | 235 | 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 |
| mathieulegros | 0:a2a44c043932 | 236 | 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 |
| mathieulegros | 0:a2a44c043932 | 237 | 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 |
| mathieulegros | 0:a2a44c043932 | 238 | 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 |
| mathieulegros | 0:a2a44c043932 | 239 | 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 |
| mathieulegros | 0:a2a44c043932 | 240 | 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ |
| mathieulegros | 0:a2a44c043932 | 241 | |
| ConstantMBED | 1:b3ff77670606 | 242 | /* NUMERO AX12 */ |
| ConstantMBED | 1:b3ff77670606 | 243 | |
| ConstantMBED | 1:b3ff77670606 | 244 | /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 |
| ConstantMBED | 1:b3ff77670606 | 245 | 1 = 0x01 | 10 = 0x0A |
| ConstantMBED | 1:b3ff77670606 | 246 | 2 = 0x02 | 11 = 0x0B |
| ConstantMBED | 1:b3ff77670606 | 247 | 3 = 0x03 | 12 = 0x0C |
| ConstantMBED | 1:b3ff77670606 | 248 | 4 = 0x04 | 13 = 0x0D |
| ConstantMBED | 1:b3ff77670606 | 249 | 5 = 0x05 | 14 = 0x0E |
| ConstantMBED | 1:b3ff77670606 | 250 | 6 = 0x06 | 15 = 0x0F |
| ConstantMBED | 1:b3ff77670606 | 251 | 7 = 0x07 | 16 = 0x10 |
| ConstantMBED | 1:b3ff77670606 | 252 | 8 = 0x08 | 17 = 0x11 */ |
| mathieulegros | 0:a2a44c043932 | 253 | |
| mathieulegros | 0:a2a44c043932 | 254 | |
| SquirrelGod | 2:99b1cb0d9f5e | 255 | |
| SquirrelGod | 2:99b1cb0d9f5e | 256 | /* MAIN */ |
| mathieulegros | 0:a2a44c043932 | 257 | |
| SquirrelGod | 2:99b1cb0d9f5e | 258 | int main() |
| mathieulegros | 0:a2a44c043932 | 259 | { |
| SquirrelGod | 2:99b1cb0d9f5e | 260 | can1.frequency(1000000); // fréquence de travail 1Mbit/s |
| SquirrelGod | 2:99b1cb0d9f5e | 261 | can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN |
| SquirrelGod | 2:99b1cb0d9f5e | 262 | |
| SquirrelGod | 4:888b32bc5b63 | 263 | //Pince centrale |
| SquirrelGod | 4:888b32bc5b63 | 264 | un_myAX12 = new AX12(p9, p10, 1, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 265 | sept_myAX12 = new AX12(p9, p10, 7, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 266 | huit_myAX12 = new AX12(p9, p10, 8, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 267 | seize_myAX12 = new AX12(p9, p10, 16, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 268 | multiple_myAX12 = new AX12(p9,p10,0xFE,1000000); |
| SquirrelGod | 4:888b32bc5b63 | 269 | |
| SquirrelGod | 4:888b32bc5b63 | 270 | //Bras de gauche |
| SquirrelGod | 4:888b32bc5b63 | 271 | un_myAX12_2 = new AX12(p13, p14, 1, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 272 | deux_myAX12_2 = new AX12(p13, p14, 2, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 273 | trois_myAX12_2 = new AX12(p13, p14, 3, 1000000); |
| SquirrelGod | 4:888b32bc5b63 | 274 | multiple_myAX12_2 = new AX12(p13,p14,0xFE,1000000); |
| SquirrelGod | 4:888b32bc5b63 | 275 | |
| SquirrelGod | 4:888b32bc5b63 | 276 | //Demonstration pince centrale |
| SquirrelGod | 4:888b32bc5b63 | 277 | /* |
| SquirrelGod | 3:6cbd7d2c78f4 | 278 | Initialisation_position(); |
| SquirrelGod | 4:888b32bc5b63 | 279 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 280 | Preparation_prise(); |
| SquirrelGod | 4:888b32bc5b63 | 281 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 282 | Stockage_haut(); |
| SquirrelGod | 4:888b32bc5b63 | 283 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 284 | Preparation_prise(); |
| SquirrelGod | 4:888b32bc5b63 | 285 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 286 | Stockage_bas(); |
| SquirrelGod | 4:888b32bc5b63 | 287 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 288 | Pousser_module(); |
| SquirrelGod | 4:888b32bc5b63 | 289 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 290 | Preparation_depot_bas(); |
| SquirrelGod | 4:888b32bc5b63 | 291 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 292 | Deposer(); |
| SquirrelGod | 4:888b32bc5b63 | 293 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 294 | Preparation_depot_haut(); |
| SquirrelGod | 4:888b32bc5b63 | 295 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 296 | Deposer(); |
| SquirrelGod | 4:888b32bc5b63 | 297 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 298 | */ |
| SquirrelGod | 4:888b32bc5b63 | 299 | |
| SquirrelGod | 4:888b32bc5b63 | 300 | //Demonstration bras gauche |
| SquirrelGod | 4:888b32bc5b63 | 301 | Initialisation_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 302 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 303 | Preparation_prise_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 304 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 305 | Prendre_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 306 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 307 | Preparation_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 308 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 309 | Tourner_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 310 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 311 | Preparation_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 312 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 313 | Tourner_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 314 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 315 | Preparation_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 316 | wait(1); |
| SquirrelGod | 4:888b32bc5b63 | 317 | Tourner_module_gauche(); |
| SquirrelGod | 4:888b32bc5b63 | 318 | |
| SquirrelGod | 2:99b1cb0d9f5e | 319 | while(true) { |
| SquirrelGod | 4:888b32bc5b63 | 320 | //AX12_automate(); |
| SquirrelGod | 2:99b1cb0d9f5e | 321 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 322 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 323 | |
| SquirrelGod | 2:99b1cb0d9f5e | 324 | /* FONCTIONS */ |
| SquirrelGod | 2:99b1cb0d9f5e | 325 | |
| SquirrelGod | 2:99b1cb0d9f5e | 326 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 327 | /* FUNCTION NAME: canProcessRx */ |
| SquirrelGod | 2:99b1cb0d9f5e | 328 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
| SquirrelGod | 2:99b1cb0d9f5e | 329 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 330 | void canProcessRx(void) |
| SquirrelGod | 2:99b1cb0d9f5e | 331 | { |
| SquirrelGod | 2:99b1cb0d9f5e | 332 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
| SquirrelGod | 4:888b32bc5b63 | 333 | |
| SquirrelGod | 2:99b1cb0d9f5e | 334 | CANMessage msgTx=CANMessage(); |
| SquirrelGod | 4:888b32bc5b63 | 335 | msgTx.format=CANStandard; |
| SquirrelGod | 4:888b32bc5b63 | 336 | msgTx.type=CANData; |
| SquirrelGod | 2:99b1cb0d9f5e | 337 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
| SquirrelGod | 4:888b32bc5b63 | 338 | |
| SquirrelGod | 2:99b1cb0d9f5e | 339 | if(FIFO_occupation<0) |
| SquirrelGod | 2:99b1cb0d9f5e | 340 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
| SquirrelGod | 4:888b32bc5b63 | 341 | |
| SquirrelGod | 2:99b1cb0d9f5e | 342 | if(FIFO_max_occupation<FIFO_occupation) |
| SquirrelGod | 2:99b1cb0d9f5e | 343 | FIFO_max_occupation=FIFO_occupation; |
| SquirrelGod | 4:888b32bc5b63 | 344 | |
| SquirrelGod | 2:99b1cb0d9f5e | 345 | if(FIFO_occupation!=0) { |
| SquirrelGod | 2:99b1cb0d9f5e | 346 | |
| SquirrelGod | 2:99b1cb0d9f5e | 347 | switch(msgRxBuffer[FIFO_lecture].id) { |
| SquirrelGod | 4:888b32bc5b63 | 348 | case CHECK_AX12: |
| SquirrelGod | 4:888b32bc5b63 | 349 | SendRawId(ALIVE_AX12); |
| SquirrelGod | 4:888b32bc5b63 | 350 | flag = 1; |
| SquirrelGod | 2:99b1cb0d9f5e | 351 | break; |
| SquirrelGod | 4:888b32bc5b63 | 352 | |
| SquirrelGod | 4:888b32bc5b63 | 353 | case SERVO_AX12_ACTION : |
| SquirrelGod | 4:888b32bc5b63 | 354 | etat_ax12 = msgRxBuffer[FIFO_lecture].data[0]; |
| SquirrelGod | 2:99b1cb0d9f5e | 355 | |
| SquirrelGod | 4:888b32bc5b63 | 356 | //ACK de reception des actions a effectuer |
| SquirrelGod | 4:888b32bc5b63 | 357 | msgTx.id = SERVO_AX12_ACK; |
| SquirrelGod | 4:888b32bc5b63 | 358 | msgTx.len = 1; |
| SquirrelGod | 4:888b32bc5b63 | 359 | msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; |
| SquirrelGod | 4:888b32bc5b63 | 360 | can1.write(msgTx); |
| SquirrelGod | 2:99b1cb0d9f5e | 361 | break; |
| SquirrelGod | 2:99b1cb0d9f5e | 362 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 363 | |
| SquirrelGod | 2:99b1cb0d9f5e | 364 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
| SquirrelGod | 2:99b1cb0d9f5e | 365 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 366 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 367 | |
| SquirrelGod | 2:99b1cb0d9f5e | 368 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 369 | /* FUNCTION NAME: canRx_ISR */ |
| SquirrelGod | 2:99b1cb0d9f5e | 370 | /* DESCRIPTION : Interruption en réception sur le CAN */ |
| SquirrelGod | 2:99b1cb0d9f5e | 371 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 372 | void canRx_ISR (void) |
| SquirrelGod | 2:99b1cb0d9f5e | 373 | { |
| SquirrelGod | 2:99b1cb0d9f5e | 374 | if (can1.read(msgRxBuffer[FIFO_ecriture])) { |
| SquirrelGod | 2:99b1cb0d9f5e | 375 | if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); |
| SquirrelGod | 2:99b1cb0d9f5e | 376 | else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; |
| SquirrelGod | 2:99b1cb0d9f5e | 377 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 378 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 379 | |
| SquirrelGod | 2:99b1cb0d9f5e | 380 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 381 | /* FUNCTION NAME: SendRawId */ |
| SquirrelGod | 2:99b1cb0d9f5e | 382 | /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ |
| SquirrelGod | 2:99b1cb0d9f5e | 383 | /****************************************************************************************/ |
| SquirrelGod | 2:99b1cb0d9f5e | 384 | void SendRawId (unsigned short id) |
| SquirrelGod | 2:99b1cb0d9f5e | 385 | { |
| SquirrelGod | 2:99b1cb0d9f5e | 386 | CANMessage msgTx=CANMessage(); |
| SquirrelGod | 2:99b1cb0d9f5e | 387 | msgTx.id=id; |
| SquirrelGod | 2:99b1cb0d9f5e | 388 | msgTx.len=0; |
| SquirrelGod | 2:99b1cb0d9f5e | 389 | can1.write(msgTx); |
| SquirrelGod | 2:99b1cb0d9f5e | 390 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 391 | void Init_AX12() // Initialisation des différents paramétres |
| SquirrelGod | 2:99b1cb0d9f5e | 392 | { |
| SquirrelGod | 4:888b32bc5b63 | 393 | un_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
| SquirrelGod | 4:888b32bc5b63 | 394 | sept_myAX12-> Set_Goal_speed(vitesse); |
| SquirrelGod | 2:99b1cb0d9f5e | 395 | huit_myAX12-> Set_Goal_speed(vitesse); |
| SquirrelGod | 4:888b32bc5b63 | 396 | seize_myAX12-> Set_Goal_speed(vitesse); |
| mathieulegros | 0:a2a44c043932 | 397 | |
| SquirrelGod | 4:888b32bc5b63 | 398 | un_myAX12-> Set_Mode(0); |
| ConstantMBED | 1:b3ff77670606 | 399 | sept_myAX12-> Set_Mode(0); |
| SquirrelGod | 4:888b32bc5b63 | 400 | huit_myAX12-> Set_Mode(0); |
| SquirrelGod | 4:888b32bc5b63 | 401 | seize_myAX12-> Set_Mode(0); |
| SquirrelGod | 2:99b1cb0d9f5e | 402 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 403 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 404 | |
| SquirrelGod | 2:99b1cb0d9f5e | 405 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 406 | /* FUNCTION NAME: Initialisation_position */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 407 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 408 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 409 | void Initialisation_position(void){ |
| SquirrelGod | 4:888b32bc5b63 | 410 | multiple_myAX12->multiple_goal_and_speed(4,TAB12); |
| SquirrelGod | 4:888b32bc5b63 | 411 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 412 | multiple_myAX12->multiple_goal_and_speed(4,TAB1); |
| SquirrelGod | 4:888b32bc5b63 | 413 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 414 | } |
| SquirrelGod | 3:6cbd7d2c78f4 | 415 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 416 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 417 | /* FUNCTION NAME: Preparation_prise */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 418 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 419 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 420 | void Preparation_prise(void){ |
| SquirrelGod | 4:888b32bc5b63 | 421 | if (action_precedente == 0){ |
| SquirrelGod | 4:888b32bc5b63 | 422 | multiple_myAX12->multiple_goal_and_speed(4,TAB12); |
| SquirrelGod | 2:99b1cb0d9f5e | 423 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 424 | action_precedente = 1; |
| SquirrelGod | 3:6cbd7d2c78f4 | 425 | } |
| SquirrelGod | 4:888b32bc5b63 | 426 | multiple_myAX12->multiple_goal_and_speed(4,TAB2); |
| SquirrelGod | 4:888b32bc5b63 | 427 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 428 | } |
| SquirrelGod | 3:6cbd7d2c78f4 | 429 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 430 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 431 | /* FUNCTION NAME: Stockage_haut */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 432 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 433 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 434 | void Stockage_haut(void){ |
| SquirrelGod | 4:888b32bc5b63 | 435 | multiple_myAX12->multiple_goal_and_speed(4,TAB3); |
| SquirrelGod | 4:888b32bc5b63 | 436 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 437 | multiple_myAX12->multiple_goal_and_speed(4,TAB4); |
| SquirrelGod | 4:888b32bc5b63 | 438 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 439 | multiple_myAX12->multiple_goal_and_speed(4,TAB5); |
| SquirrelGod | 4:888b32bc5b63 | 440 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 441 | multiple_myAX12->multiple_goal_and_speed(4,TAB6); |
| SquirrelGod | 4:888b32bc5b63 | 442 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 443 | } |
| SquirrelGod | 3:6cbd7d2c78f4 | 444 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 445 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 446 | /* FUNCTION NAME: Stockage_bas */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 447 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 448 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 449 | void Stockage_bas(void){ |
| SquirrelGod | 4:888b32bc5b63 | 450 | multiple_myAX12->multiple_goal_and_speed(4,TAB3); |
| SquirrelGod | 4:888b32bc5b63 | 451 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 452 | multiple_myAX12->multiple_goal_and_speed(4,TAB7); |
| SquirrelGod | 4:888b32bc5b63 | 453 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 454 | } |
| SquirrelGod | 3:6cbd7d2c78f4 | 455 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 456 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 457 | /* FUNCTION NAME: Deposer */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 458 | /* DESCRIPTION : Fonction qui permet de déposer le module */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 459 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 460 | void Deposer(void){ |
| SquirrelGod | 4:888b32bc5b63 | 461 | multiple_myAX12->multiple_goal_and_speed(4,TAB9); |
| SquirrelGod | 4:888b32bc5b63 | 462 | wait(TIME/5); |
| SquirrelGod | 4:888b32bc5b63 | 463 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
| SquirrelGod | 4:888b32bc5b63 | 464 | wait(TIME/5); |
| SquirrelGod | 4:888b32bc5b63 | 465 | multiple_myAX12->multiple_goal_and_speed(4,TAB13); |
| SquirrelGod | 4:888b32bc5b63 | 466 | wait(TIME/5); |
| SquirrelGod | 2:99b1cb0d9f5e | 467 | } |
| mathieulegros | 0:a2a44c043932 | 468 | |
| SquirrelGod | 2:99b1cb0d9f5e | 469 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 470 | /* FUNCTION NAME: Preparation_depot_bas */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 471 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
| SquirrelGod | 2:99b1cb0d9f5e | 472 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 473 | void Preparation_depot_bas(void){ |
| SquirrelGod | 4:888b32bc5b63 | 474 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
| SquirrelGod | 4:888b32bc5b63 | 475 | wait(TIME); |
| SquirrelGod | 2:99b1cb0d9f5e | 476 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 477 | |
| SquirrelGod | 2:99b1cb0d9f5e | 478 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 479 | /* FUNCTION NAME: Preparation_depot_haut */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 480 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
| SquirrelGod | 2:99b1cb0d9f5e | 481 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 482 | void Preparation_depot_haut(void){ |
| SquirrelGod | 4:888b32bc5b63 | 483 | multiple_myAX12->multiple_goal_and_speed(4,TAB6); |
| SquirrelGod | 4:888b32bc5b63 | 484 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 485 | multiple_myAX12->multiple_goal_and_speed(4,TAB5); |
| SquirrelGod | 4:888b32bc5b63 | 486 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 487 | multiple_myAX12->multiple_goal_and_speed(4,TAB10); |
| SquirrelGod | 4:888b32bc5b63 | 488 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 489 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
| SquirrelGod | 4:888b32bc5b63 | 490 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 491 | } |
| SquirrelGod | 2:99b1cb0d9f5e | 492 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 493 | /****************************************************************************************/ |
| SquirrelGod | 3:6cbd7d2c78f4 | 494 | /* FUNCTION NAME: Pousser_module */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 495 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
| SquirrelGod | 3:6cbd7d2c78f4 | 496 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 497 | void Pousser_module(void){ |
| SquirrelGod | 4:888b32bc5b63 | 498 | multiple_myAX12->multiple_goal_and_speed(4,TAB11); |
| SquirrelGod | 4:888b32bc5b63 | 499 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 500 | } |
| SquirrelGod | 4:888b32bc5b63 | 501 | |
| SquirrelGod | 4:888b32bc5b63 | 502 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 503 | /* FUNCTION NAME: Initialisation_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 504 | /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ |
| SquirrelGod | 4:888b32bc5b63 | 505 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 506 | void Initialisation_gauche(void){ |
| SquirrelGod | 4:888b32bc5b63 | 507 | trois_myAX12_2->Set_Secure_Goal(235); |
| SquirrelGod | 4:888b32bc5b63 | 508 | multiple_myAX12_2->multiple_goal_and_speed(4,TAB22); |
| SquirrelGod | 4:888b32bc5b63 | 509 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 510 | multiple_myAX12_2->multiple_goal_and_speed(4,TAB21); |
| SquirrelGod | 4:888b32bc5b63 | 511 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 512 | } |
| SquirrelGod | 4:888b32bc5b63 | 513 | |
| SquirrelGod | 4:888b32bc5b63 | 514 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 515 | /* FUNCTION NAME: Preparation_prise_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 516 | /* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */ |
| SquirrelGod | 4:888b32bc5b63 | 517 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 518 | void Preparation_prise_gauche(void){ |
| SquirrelGod | 4:888b32bc5b63 | 519 | trois_myAX12_2->Set_Secure_Goal(120); |
| SquirrelGod | 4:888b32bc5b63 | 520 | } |
| SquirrelGod | 4:888b32bc5b63 | 521 | |
| SquirrelGod | 4:888b32bc5b63 | 522 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 523 | /* FUNCTION NAME: Prendre_module_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 524 | /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 525 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 526 | void Prendre_module_gauche(void){ |
| SquirrelGod | 4:888b32bc5b63 | 527 | trois_myAX12_2->Set_Secure_Goal(160); |
| SquirrelGod | 4:888b32bc5b63 | 528 | } |
| SquirrelGod | 4:888b32bc5b63 | 529 | |
| SquirrelGod | 4:888b32bc5b63 | 530 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 531 | /* FUNCTION NAME: Preparatio_module_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 532 | /* DESCRIPTION : Fonction qui prepare le tournante */ |
| SquirrelGod | 4:888b32bc5b63 | 533 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 534 | void Preparation_module_gauche(void){ |
| SquirrelGod | 4:888b32bc5b63 | 535 | multiple_myAX12_2->multiple_goal_and_speed(4,TAB22); |
| SquirrelGod | 4:888b32bc5b63 | 536 | wait(TIME); |
| SquirrelGod | 4:888b32bc5b63 | 537 | } |
| SquirrelGod | 4:888b32bc5b63 | 538 | |
| SquirrelGod | 4:888b32bc5b63 | 539 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 540 | /* FUNCTION NAME: Tourner_module_gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 541 | /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ |
| SquirrelGod | 4:888b32bc5b63 | 542 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 543 | void Tourner_module_gauche(void){ |
| SquirrelGod | 4:888b32bc5b63 | 544 | multiple_myAX12_2->multiple_goal_and_speed(4,TAB23); |
| SquirrelGod | 4:888b32bc5b63 | 545 | wait(TIME); |
| SquirrelGod | 3:6cbd7d2c78f4 | 546 | } |
| SquirrelGod | 3:6cbd7d2c78f4 | 547 | |
| SquirrelGod | 3:6cbd7d2c78f4 | 548 | |
| SquirrelGod | 4:888b32bc5b63 | 549 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 550 | /* FUNCTION NAME: Fin_action */ |
| SquirrelGod | 4:888b32bc5b63 | 551 | /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ |
| SquirrelGod | 4:888b32bc5b63 | 552 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 553 | void Fin_action(void){ |
| SquirrelGod | 4:888b32bc5b63 | 554 | CANMessage msgTx=CANMessage(); |
| SquirrelGod | 4:888b32bc5b63 | 555 | msgTx.format=CANStandard; |
| SquirrelGod | 4:888b32bc5b63 | 556 | msgTx.type=CANData; |
| SquirrelGod | 4:888b32bc5b63 | 557 | |
| SquirrelGod | 4:888b32bc5b63 | 558 | msgTx.id = SERVO_AX12_END; |
| SquirrelGod | 4:888b32bc5b63 | 559 | msgTx.len = 1; |
| SquirrelGod | 4:888b32bc5b63 | 560 | msgTx.data[0] = AX12_PREPARATION_PRISE; |
| SquirrelGod | 4:888b32bc5b63 | 561 | can1.write(msgTx); |
| SquirrelGod | 4:888b32bc5b63 | 562 | } |
| SquirrelGod | 3:6cbd7d2c78f4 | 563 | |
| SquirrelGod | 4:888b32bc5b63 | 564 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 565 | /* FUNCTION NAME: Automate_ax12 */ |
| SquirrelGod | 4:888b32bc5b63 | 566 | /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ |
| SquirrelGod | 4:888b32bc5b63 | 567 | /****************************************************************************************/ |
| SquirrelGod | 4:888b32bc5b63 | 568 | void AX12_automate(void){ |
| SquirrelGod | 4:888b32bc5b63 | 569 | switch(etat_ax12){ |
| SquirrelGod | 4:888b32bc5b63 | 570 | case AX12_INITIALISATION : |
| SquirrelGod | 4:888b32bc5b63 | 571 | if (flag == 1){ |
| SquirrelGod | 4:888b32bc5b63 | 572 | Initialisation_position(); |
| SquirrelGod | 4:888b32bc5b63 | 573 | flag = 2; |
| SquirrelGod | 4:888b32bc5b63 | 574 | } |
| SquirrelGod | 4:888b32bc5b63 | 575 | |
| SquirrelGod | 4:888b32bc5b63 | 576 | break; |
| SquirrelGod | 4:888b32bc5b63 | 577 | |
| SquirrelGod | 4:888b32bc5b63 | 578 | case AX12_PREPARATION_PRISE : |
| SquirrelGod | 4:888b32bc5b63 | 579 | Preparation_prise(); |
| SquirrelGod | 4:888b32bc5b63 | 580 | if (action == 0){ |
| SquirrelGod | 4:888b32bc5b63 | 581 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 582 | action ++; |
| SquirrelGod | 4:888b32bc5b63 | 583 | } |
| SquirrelGod | 4:888b32bc5b63 | 584 | break; |
| SquirrelGod | 4:888b32bc5b63 | 585 | |
| SquirrelGod | 4:888b32bc5b63 | 586 | case AX12_STOCKAGE_HAUT : |
| SquirrelGod | 4:888b32bc5b63 | 587 | Stockage_haut(); |
| SquirrelGod | 4:888b32bc5b63 | 588 | etat_ax12 = AX12_DEFAUT; |
| SquirrelGod | 4:888b32bc5b63 | 589 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 590 | break; |
| SquirrelGod | 4:888b32bc5b63 | 591 | |
| SquirrelGod | 4:888b32bc5b63 | 592 | case AX12_STOCKAGE_BAS : |
| SquirrelGod | 4:888b32bc5b63 | 593 | Stockage_bas(); |
| SquirrelGod | 4:888b32bc5b63 | 594 | etat_ax12 = AX12_DEFAUT; |
| SquirrelGod | 4:888b32bc5b63 | 595 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 596 | break; |
| SquirrelGod | 4:888b32bc5b63 | 597 | |
| SquirrelGod | 4:888b32bc5b63 | 598 | case AX12_DEPOSER : |
| SquirrelGod | 4:888b32bc5b63 | 599 | Deposer(); |
| SquirrelGod | 4:888b32bc5b63 | 600 | etat_ax12 = AX12_DEFAUT; |
| SquirrelGod | 4:888b32bc5b63 | 601 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 602 | break; |
| SquirrelGod | 4:888b32bc5b63 | 603 | |
| SquirrelGod | 4:888b32bc5b63 | 604 | case AX12_PREPARATION_DEPOT_BAS : |
| SquirrelGod | 4:888b32bc5b63 | 605 | Preparation_depot_bas(); |
| SquirrelGod | 4:888b32bc5b63 | 606 | etat_ax12 = AX12_DEFAUT; |
| SquirrelGod | 4:888b32bc5b63 | 607 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 608 | break; |
| SquirrelGod | 4:888b32bc5b63 | 609 | |
| SquirrelGod | 4:888b32bc5b63 | 610 | case AX12_PREPARATION_DEPOT_HAUT : |
| SquirrelGod | 4:888b32bc5b63 | 611 | Preparation_depot_haut(); |
| SquirrelGod | 4:888b32bc5b63 | 612 | etat_ax12 = AX12_DEFAUT; |
| SquirrelGod | 4:888b32bc5b63 | 613 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 614 | break; |
| SquirrelGod | 4:888b32bc5b63 | 615 | |
| SquirrelGod | 4:888b32bc5b63 | 616 | case AX12_POUSSER_MODULE : |
| SquirrelGod | 4:888b32bc5b63 | 617 | Pousser_module(); |
| SquirrelGod | 4:888b32bc5b63 | 618 | etat_ax12 = AX12_DEFAUT; |
| SquirrelGod | 4:888b32bc5b63 | 619 | Fin_action(); |
| SquirrelGod | 4:888b32bc5b63 | 620 | break; |
| SquirrelGod | 4:888b32bc5b63 | 621 | |
| SquirrelGod | 4:888b32bc5b63 | 622 | case AX12_DEFAUT : |
| SquirrelGod | 4:888b32bc5b63 | 623 | action = 0; |
| SquirrelGod | 4:888b32bc5b63 | 624 | break; |
| SquirrelGod | 4:888b32bc5b63 | 625 | } |
| SquirrelGod | 4:888b32bc5b63 | 626 | } |