mouvement AX12 petit robot
Fork of command_AX12 by
main.cpp@3:6cbd7d2c78f4, 2017-04-19 (annotated)
- Committer:
- SquirrelGod
- Date:
- Wed Apr 19 15:53:45 2017 +0000
- Revision:
- 3:6cbd7d2c78f4
- Parent:
- 2:99b1cb0d9f5e
- Child:
- 4:888b32bc5b63
AX12 control
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 | 2:99b1cb0d9f5e | 6 | #define SIZE_FIFO 14 |
SquirrelGod | 2:99b1cb0d9f5e | 7 | #define TIME 0.8 |
SquirrelGod | 2:99b1cb0d9f5e | 8 | |
mathieulegros | 0:a2a44c043932 | 9 | /* DECLARATION VARIABLES */ |
SquirrelGod | 2:99b1cb0d9f5e | 10 | CAN can1(p30,p29); |
SquirrelGod | 2:99b1cb0d9f5e | 11 | CANMessage msgRxBuffer[SIZE_FIFO]; |
SquirrelGod | 2:99b1cb0d9f5e | 12 | unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN |
SquirrelGod | 2:99b1cb0d9f5e | 13 | unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN |
SquirrelGod | 2:99b1cb0d9f5e | 14 | |
SquirrelGod | 2:99b1cb0d9f5e | 15 | extern "C" void mbed_reset();//Pour pouvoir reset la carte |
SquirrelGod | 2:99b1cb0d9f5e | 16 | |
SquirrelGod | 2:99b1cb0d9f5e | 17 | unsigned char action = 0, choix = 0; |
SquirrelGod | 2:99b1cb0d9f5e | 18 | short vitesse=700; |
SquirrelGod | 2:99b1cb0d9f5e | 19 | float angle=0.0; |
SquirrelGod | 2:99b1cb0d9f5e | 20 | 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 | 3:6cbd7d2c78f4 | 21 | AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; |
SquirrelGod | 2:99b1cb0d9f5e | 22 | |
SquirrelGod | 2:99b1cb0d9f5e | 23 | |
SquirrelGod | 2:99b1cb0d9f5e | 24 | /* PROTOTYPES DE FONCTIONS ET POINTEURS */ |
SquirrelGod | 2:99b1cb0d9f5e | 25 | |
SquirrelGod | 2:99b1cb0d9f5e | 26 | |
SquirrelGod | 2:99b1cb0d9f5e | 27 | |
SquirrelGod | 2:99b1cb0d9f5e | 28 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 29 | /* FUNCTION NAME: canProcessRx */ |
SquirrelGod | 2:99b1cb0d9f5e | 30 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 31 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 32 | void canProcessRx(void); |
SquirrelGod | 2:99b1cb0d9f5e | 33 | |
SquirrelGod | 2:99b1cb0d9f5e | 34 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 35 | /* FUNCTION NAME: canRx_ISR */ |
SquirrelGod | 2:99b1cb0d9f5e | 36 | /* DESCRIPTION : Interruption en réception sur le CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 37 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 38 | void canRx_ISR (void); |
SquirrelGod | 2:99b1cb0d9f5e | 39 | |
SquirrelGod | 2:99b1cb0d9f5e | 40 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 41 | /* FUNCTION NAME: SendRawId */ |
SquirrelGod | 2:99b1cb0d9f5e | 42 | /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ |
SquirrelGod | 2:99b1cb0d9f5e | 43 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 44 | void SendRawId (unsigned short id); |
SquirrelGod | 2:99b1cb0d9f5e | 45 | |
SquirrelGod | 2:99b1cb0d9f5e | 46 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 47 | /* FUNCTION NAME: Initialisation_position */ |
SquirrelGod | 3:6cbd7d2c78f4 | 48 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
SquirrelGod | 3:6cbd7d2c78f4 | 49 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 50 | void Initialisation_position(void); |
SquirrelGod | 3:6cbd7d2c78f4 | 51 | |
SquirrelGod | 3:6cbd7d2c78f4 | 52 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 53 | /* FUNCTION NAME: Preparation_prise */ |
SquirrelGod | 3:6cbd7d2c78f4 | 54 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
SquirrelGod | 3:6cbd7d2c78f4 | 55 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 56 | void Preparation_prise(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 57 | |
SquirrelGod | 3:6cbd7d2c78f4 | 58 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 59 | /* FUNCTION NAME: Stockage_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 60 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
SquirrelGod | 3:6cbd7d2c78f4 | 61 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 62 | void Stockage_haut(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 63 | |
SquirrelGod | 2:99b1cb0d9f5e | 64 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 65 | /* FUNCTION NAME: Stockage_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 66 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
SquirrelGod | 2:99b1cb0d9f5e | 67 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 68 | void Stockage_bas(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 69 | |
SquirrelGod | 3:6cbd7d2c78f4 | 70 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 71 | /* FUNCTION NAME: Deposer */ |
SquirrelGod | 3:6cbd7d2c78f4 | 72 | /* DESCRIPTION : Fonction qui permet de déposer un module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 73 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 74 | void Deposer(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 75 | |
SquirrelGod | 2:99b1cb0d9f5e | 76 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 77 | /* FUNCTION NAME: Preparation_depos_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 78 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
SquirrelGod | 2:99b1cb0d9f5e | 79 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 80 | void Preparation_depos_bas(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 81 | |
SquirrelGod | 2:99b1cb0d9f5e | 82 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 83 | /* FUNCTION NAME: Preparation_depos_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 84 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
SquirrelGod | 2:99b1cb0d9f5e | 85 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 86 | void Preparation_depos_haut(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 87 | |
SquirrelGod | 3:6cbd7d2c78f4 | 88 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 89 | /* FUNCTION NAME: Pousser_module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 90 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 91 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 92 | void Pousser_module(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 93 | |
mathieulegros | 0:a2a44c043932 | 94 | Timer t; |
mathieulegros | 0:a2a44c043932 | 95 | Ticker flipper; |
SquirrelGod | 2:99b1cb0d9f5e | 96 | |
SquirrelGod | 2:99b1cb0d9f5e | 97 | ///////////TABLEAU PINCE DROITE /////////////////////////// |
SquirrelGod | 3:6cbd7d2c78f4 | 98 | static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale |
SquirrelGod | 3:6cbd7d2c78f4 | 99 | 0x04,0xF4, 0x01, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 100 | 0x07,0x50, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 101 | 0x0F,0xB0, 0x03, 0xFF, 0x00}; |
ConstantMBED | 1:b3ff77670606 | 102 | |
SquirrelGod | 3:6cbd7d2c78f4 | 103 | static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise |
SquirrelGod | 3:6cbd7d2c78f4 | 104 | 0x04,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 105 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 106 | 0x0F,0xF4, 0x01, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 107 | |
SquirrelGod | 3:6cbd7d2c78f4 | 108 | static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) |
SquirrelGod | 3:6cbd7d2c78f4 | 109 | 0x04,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 110 | 0x07,0xB5, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 111 | 0x0F,0x39, 0x03, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 112 | |
SquirrelGod | 3:6cbd7d2c78f4 | 113 | static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) |
SquirrelGod | 3:6cbd7d2c78f4 | 114 | 0x04,0x00, 0x02, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 115 | 0x07,0xB5, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 116 | 0x0F,0x39, 0x03, 0xFF, 0x03}; |
SquirrelGod | 3:6cbd7d2c78f4 | 117 | |
SquirrelGod | 3:6cbd7d2c78f4 | 118 | static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) |
SquirrelGod | 3:6cbd7d2c78f4 | 119 | 0x04,0x60, 0x01, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 120 | 0x07,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 121 | 0x0F,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 122 | |
SquirrelGod | 3:6cbd7d2c78f4 | 123 | static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 124 | 0x04,0x50, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 125 | 0x07,0x0A, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 126 | 0x0F,0xDD, 0x02, 0xFF, 0x03}; |
SquirrelGod | 3:6cbd7d2c78f4 | 127 | |
SquirrelGod | 3:6cbd7d2c78f4 | 128 | static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) |
SquirrelGod | 3:6cbd7d2c78f4 | 129 | 0x04,0xDE, 0x03, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 130 | 0x07,0xB5, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 131 | 0x0F,0x39, 0x03, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 132 | |
SquirrelGod | 3:6cbd7d2c78f4 | 133 | static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras) |
SquirrelGod | 3:6cbd7d2c78f4 | 134 | 0x04,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 135 | 0x07,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 136 | 0x0F,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 137 | |
SquirrelGod | 3:6cbd7d2c78f4 | 138 | static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 139 | 0x04,0x00, 0x03, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 140 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 141 | 0x0F,0xF4, 0x01, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 142 | |
SquirrelGod | 3:6cbd7d2c78f4 | 143 | static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut) |
SquirrelGod | 3:6cbd7d2c78f4 | 144 | 0x04,0x00, 0x02, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 145 | 0x07,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 146 | 0x0F,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 147 | |
SquirrelGod | 3:6cbd7d2c78f4 | 148 | static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) |
SquirrelGod | 3:6cbd7d2c78f4 | 149 | 0x04,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 150 | 0x07,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 151 | 0x0F,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 2:99b1cb0d9f5e | 152 | |
ConstantMBED | 1:b3ff77670606 | 153 | |
SquirrelGod | 2:99b1cb0d9f5e | 154 | //////////////////TABLEAU PINCE GAUCHE ///////////////////////////// |
SquirrelGod | 3:6cbd7d2c78f4 | 155 | static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale |
SquirrelGod | 3:6cbd7d2c78f4 | 156 | 0x03,0xF4, 0x01, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 157 | 0x0D,0x50, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 158 | 0x0E,0xB0, 0x03, 0xFF, 0x00}; |
SquirrelGod | 2:99b1cb0d9f5e | 159 | |
SquirrelGod | 3:6cbd7d2c78f4 | 160 | static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise |
SquirrelGod | 3:6cbd7d2c78f4 | 161 | 0x03,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 162 | 0x0D,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 163 | 0x0E,0xF4, 0x01, 0xFF, 0x03}; |
ConstantMBED | 1:b3ff77670606 | 164 | |
SquirrelGod | 3:6cbd7d2c78f4 | 165 | static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) |
SquirrelGod | 3:6cbd7d2c78f4 | 166 | 0x03,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 167 | 0x0D,0xB5, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 168 | 0x0E,0x39, 0x03, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 169 | |
SquirrelGod | 3:6cbd7d2c78f4 | 170 | static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) |
SquirrelGod | 3:6cbd7d2c78f4 | 171 | 0x03,0x00, 0x02, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 172 | 0x0D,0xB5, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 173 | 0x0E,0x39, 0x03, 0xFF, 0x03}; |
SquirrelGod | 3:6cbd7d2c78f4 | 174 | |
SquirrelGod | 3:6cbd7d2c78f4 | 175 | static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) |
SquirrelGod | 3:6cbd7d2c78f4 | 176 | 0x03,0x60, 0x01, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 177 | 0x0D,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 178 | 0x0E,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 179 | |
SquirrelGod | 3:6cbd7d2c78f4 | 180 | static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 181 | 0x03,0x50, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 182 | 0x0D,0x0A, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 183 | 0x0E,0xDD, 0x02, 0xFF, 0x03}; |
SquirrelGod | 3:6cbd7d2c78f4 | 184 | |
SquirrelGod | 3:6cbd7d2c78f4 | 185 | static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) |
SquirrelGod | 3:6cbd7d2c78f4 | 186 | 0x03,0xDE, 0x03, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 187 | 0x0D,0xB5, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 188 | 0x0E,0x39, 0x03, 0xFF, 0x03}; |
ConstantMBED | 1:b3ff77670606 | 189 | |
SquirrelGod | 3:6cbd7d2c78f4 | 190 | static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras) |
SquirrelGod | 3:6cbd7d2c78f4 | 191 | 0x03,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 192 | 0x0D,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 193 | 0x0E,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 194 | |
SquirrelGod | 3:6cbd7d2c78f4 | 195 | static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 196 | 0x03,0x00, 0x03, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 197 | 0x0D,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 198 | 0x0E,0xF4, 0x01, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 199 | |
SquirrelGod | 3:6cbd7d2c78f4 | 200 | static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut) |
SquirrelGod | 3:6cbd7d2c78f4 | 201 | 0x03,0x00, 0x02, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 202 | 0x0D,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 203 | 0x0E,0x39, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 204 | |
SquirrelGod | 3:6cbd7d2c78f4 | 205 | static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) |
SquirrelGod | 3:6cbd7d2c78f4 | 206 | 0x03,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 207 | 0x0D,0xB5, 0x00, 0xFF, 0x00, |
SquirrelGod | 3:6cbd7d2c78f4 | 208 | 0x0E,0x39, 0x03, 0xFF, 0x00}; |
ConstantMBED | 1:b3ff77670606 | 209 | |
SquirrelGod | 2:99b1cb0d9f5e | 210 | |
mathieulegros | 0:a2a44c043932 | 211 | /* ANGLE */ |
mathieulegros | 0:a2a44c043932 | 212 | |
mathieulegros | 0:a2a44c043932 | 213 | /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 |
mathieulegros | 0:a2a44c043932 | 214 | 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 |
mathieulegros | 0:a2a44c043932 | 215 | 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 |
mathieulegros | 0:a2a44c043932 | 216 | 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 |
mathieulegros | 0:a2a44c043932 | 217 | 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 |
mathieulegros | 0:a2a44c043932 | 218 | 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 |
mathieulegros | 0:a2a44c043932 | 219 | 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 |
mathieulegros | 0:a2a44c043932 | 220 | 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 |
mathieulegros | 0:a2a44c043932 | 221 | 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 |
mathieulegros | 0:a2a44c043932 | 222 | 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ |
mathieulegros | 0:a2a44c043932 | 223 | |
ConstantMBED | 1:b3ff77670606 | 224 | /* NUMERO AX12 */ |
ConstantMBED | 1:b3ff77670606 | 225 | |
ConstantMBED | 1:b3ff77670606 | 226 | /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 |
ConstantMBED | 1:b3ff77670606 | 227 | 1 = 0x01 | 10 = 0x0A |
ConstantMBED | 1:b3ff77670606 | 228 | 2 = 0x02 | 11 = 0x0B |
ConstantMBED | 1:b3ff77670606 | 229 | 3 = 0x03 | 12 = 0x0C |
ConstantMBED | 1:b3ff77670606 | 230 | 4 = 0x04 | 13 = 0x0D |
ConstantMBED | 1:b3ff77670606 | 231 | 5 = 0x05 | 14 = 0x0E |
ConstantMBED | 1:b3ff77670606 | 232 | 6 = 0x06 | 15 = 0x0F |
ConstantMBED | 1:b3ff77670606 | 233 | 7 = 0x07 | 16 = 0x10 |
ConstantMBED | 1:b3ff77670606 | 234 | 8 = 0x08 | 17 = 0x11 */ |
mathieulegros | 0:a2a44c043932 | 235 | |
mathieulegros | 0:a2a44c043932 | 236 | |
SquirrelGod | 2:99b1cb0d9f5e | 237 | |
SquirrelGod | 2:99b1cb0d9f5e | 238 | /* MAIN */ |
mathieulegros | 0:a2a44c043932 | 239 | |
SquirrelGod | 2:99b1cb0d9f5e | 240 | int main() |
mathieulegros | 0:a2a44c043932 | 241 | { |
SquirrelGod | 2:99b1cb0d9f5e | 242 | can1.frequency(1000000); // fréquence de travail 1Mbit/s |
SquirrelGod | 2:99b1cb0d9f5e | 243 | can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN |
SquirrelGod | 2:99b1cb0d9f5e | 244 | |
SquirrelGod | 2:99b1cb0d9f5e | 245 | CANMessage TrameTx = CANMessage(); |
SquirrelGod | 2:99b1cb0d9f5e | 246 | TrameTx.format=CANStandard; |
SquirrelGod | 2:99b1cb0d9f5e | 247 | TrameTx.type=CANData; |
SquirrelGod | 3:6cbd7d2c78f4 | 248 | |
SquirrelGod | 3:6cbd7d2c78f4 | 249 | trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche |
SquirrelGod | 3:6cbd7d2c78f4 | 250 | treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche |
SquirrelGod | 3:6cbd7d2c78f4 | 251 | quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche |
SquirrelGod | 3:6cbd7d2c78f4 | 252 | huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche |
SquirrelGod | 2:99b1cb0d9f5e | 253 | |
SquirrelGod | 3:6cbd7d2c78f4 | 254 | quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 255 | sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 256 | quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 257 | dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 258 | |
SquirrelGod | 2:99b1cb0d9f5e | 259 | multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); |
SquirrelGod | 2:99b1cb0d9f5e | 260 | multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); |
SquirrelGod | 2:99b1cb0d9f5e | 261 | |
SquirrelGod | 3:6cbd7d2c78f4 | 262 | //Initialisation de la position du bras |
SquirrelGod | 3:6cbd7d2c78f4 | 263 | Initialisation_position(); |
SquirrelGod | 2:99b1cb0d9f5e | 264 | |
SquirrelGod | 2:99b1cb0d9f5e | 265 | while(true) { |
SquirrelGod | 2:99b1cb0d9f5e | 266 | canProcessRx();//Traitement des trames CAN en attente |
SquirrelGod | 2:99b1cb0d9f5e | 267 | |
SquirrelGod | 2:99b1cb0d9f5e | 268 | if (action == 0){ |
SquirrelGod | 3:6cbd7d2c78f4 | 269 | |
SquirrelGod | 2:99b1cb0d9f5e | 270 | /* |
SquirrelGod | 2:99b1cb0d9f5e | 271 | TrameTx.id = 0x092; |
SquirrelGod | 2:99b1cb0d9f5e | 272 | TrameTx.len = 1; |
SquirrelGod | 2:99b1cb0d9f5e | 273 | TrameTx.data[0] = 0; |
SquirrelGod | 2:99b1cb0d9f5e | 274 | can1.write(TrameTx); |
SquirrelGod | 2:99b1cb0d9f5e | 275 | */ |
SquirrelGod | 2:99b1cb0d9f5e | 276 | |
SquirrelGod | 2:99b1cb0d9f5e | 277 | action = 2; |
SquirrelGod | 2:99b1cb0d9f5e | 278 | } |
SquirrelGod | 2:99b1cb0d9f5e | 279 | } |
SquirrelGod | 2:99b1cb0d9f5e | 280 | } |
SquirrelGod | 2:99b1cb0d9f5e | 281 | |
SquirrelGod | 2:99b1cb0d9f5e | 282 | /* FONCTIONS */ |
SquirrelGod | 2:99b1cb0d9f5e | 283 | |
SquirrelGod | 2:99b1cb0d9f5e | 284 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 285 | /* FUNCTION NAME: canProcessRx */ |
SquirrelGod | 2:99b1cb0d9f5e | 286 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 287 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 288 | void canProcessRx(void) |
SquirrelGod | 2:99b1cb0d9f5e | 289 | { |
SquirrelGod | 2:99b1cb0d9f5e | 290 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
SquirrelGod | 2:99b1cb0d9f5e | 291 | CANMessage msgTx=CANMessage(); |
SquirrelGod | 2:99b1cb0d9f5e | 292 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
SquirrelGod | 2:99b1cb0d9f5e | 293 | if(FIFO_occupation<0) |
SquirrelGod | 2:99b1cb0d9f5e | 294 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
SquirrelGod | 2:99b1cb0d9f5e | 295 | if(FIFO_max_occupation<FIFO_occupation) |
SquirrelGod | 2:99b1cb0d9f5e | 296 | FIFO_max_occupation=FIFO_occupation; |
SquirrelGod | 2:99b1cb0d9f5e | 297 | if(FIFO_occupation!=0) { |
SquirrelGod | 2:99b1cb0d9f5e | 298 | |
SquirrelGod | 2:99b1cb0d9f5e | 299 | switch(msgRxBuffer[FIFO_lecture].id) { |
SquirrelGod | 2:99b1cb0d9f5e | 300 | case SERVO_AX12_PROCESS : |
SquirrelGod | 2:99b1cb0d9f5e | 301 | action = 1; |
SquirrelGod | 2:99b1cb0d9f5e | 302 | break; |
SquirrelGod | 2:99b1cb0d9f5e | 303 | |
SquirrelGod | 2:99b1cb0d9f5e | 304 | case CHECK_ACTIONNEURS: |
SquirrelGod | 2:99b1cb0d9f5e | 305 | SendRawId (ALIVE_ACTIONNEURS); |
SquirrelGod | 2:99b1cb0d9f5e | 306 | break; |
SquirrelGod | 2:99b1cb0d9f5e | 307 | |
SquirrelGod | 2:99b1cb0d9f5e | 308 | } |
SquirrelGod | 2:99b1cb0d9f5e | 309 | |
SquirrelGod | 2:99b1cb0d9f5e | 310 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
SquirrelGod | 2:99b1cb0d9f5e | 311 | } |
SquirrelGod | 2:99b1cb0d9f5e | 312 | } |
SquirrelGod | 2:99b1cb0d9f5e | 313 | |
SquirrelGod | 2:99b1cb0d9f5e | 314 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 315 | /* FUNCTION NAME: canRx_ISR */ |
SquirrelGod | 2:99b1cb0d9f5e | 316 | /* DESCRIPTION : Interruption en réception sur le CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 317 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 318 | void canRx_ISR (void) |
SquirrelGod | 2:99b1cb0d9f5e | 319 | { |
SquirrelGod | 2:99b1cb0d9f5e | 320 | if (can1.read(msgRxBuffer[FIFO_ecriture])) { |
SquirrelGod | 2:99b1cb0d9f5e | 321 | if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); |
SquirrelGod | 2:99b1cb0d9f5e | 322 | else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; |
SquirrelGod | 2:99b1cb0d9f5e | 323 | } |
SquirrelGod | 2:99b1cb0d9f5e | 324 | } |
SquirrelGod | 2:99b1cb0d9f5e | 325 | |
SquirrelGod | 2:99b1cb0d9f5e | 326 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 327 | /* FUNCTION NAME: SendRawId */ |
SquirrelGod | 2:99b1cb0d9f5e | 328 | /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ |
SquirrelGod | 2:99b1cb0d9f5e | 329 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 330 | void SendRawId (unsigned short id) |
SquirrelGod | 2:99b1cb0d9f5e | 331 | { |
SquirrelGod | 2:99b1cb0d9f5e | 332 | CANMessage msgTx=CANMessage(); |
SquirrelGod | 2:99b1cb0d9f5e | 333 | msgTx.id=id; |
SquirrelGod | 2:99b1cb0d9f5e | 334 | msgTx.len=0; |
SquirrelGod | 2:99b1cb0d9f5e | 335 | can1.write(msgTx); |
SquirrelGod | 2:99b1cb0d9f5e | 336 | } |
SquirrelGod | 2:99b1cb0d9f5e | 337 | void Init_AX12() // Initialisation des différents paramétres |
SquirrelGod | 2:99b1cb0d9f5e | 338 | { |
SquirrelGod | 2:99b1cb0d9f5e | 339 | deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
SquirrelGod | 2:99b1cb0d9f5e | 340 | quatre_myAX12-> Set_Goal_speed(vitesse); |
ConstantMBED | 1:b3ff77670606 | 341 | sept_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 342 | quatorze_myAX12-> Set_Goal_speed(vitesse); |
ConstantMBED | 1:b3ff77670606 | 343 | quinze_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 344 | dixhuit_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 345 | huit_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 346 | trois_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 347 | quatorze_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 348 | treize_myAX12-> Set_Goal_speed(vitesse); |
mathieulegros | 0:a2a44c043932 | 349 | |
SquirrelGod | 2:99b1cb0d9f5e | 350 | deux_myAX12-> Set_Mode(0); |
ConstantMBED | 1:b3ff77670606 | 351 | quatre_myAX12-> Set_Mode(0); |
ConstantMBED | 1:b3ff77670606 | 352 | sept_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 353 | quatorze_myAX12-> Set_Mode(0); |
ConstantMBED | 1:b3ff77670606 | 354 | quinze_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 355 | dixhuit_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 356 | huit_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 357 | trois_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 358 | quatorze_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 359 | treize_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 360 | } |
SquirrelGod | 2:99b1cb0d9f5e | 361 | |
SquirrelGod | 3:6cbd7d2c78f4 | 362 | |
SquirrelGod | 2:99b1cb0d9f5e | 363 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 364 | /* FUNCTION NAME: Initialisation_position */ |
SquirrelGod | 3:6cbd7d2c78f4 | 365 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
SquirrelGod | 3:6cbd7d2c78f4 | 366 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 367 | void Initialisation_position(void){ |
SquirrelGod | 3:6cbd7d2c78f4 | 368 | multiple_myAX12->multiple_goal_and_speed(4,TAB1); |
SquirrelGod | 3:6cbd7d2c78f4 | 369 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 370 | multiple2_myAX12->multiple_goal_and_speed(4,TAB21); |
SquirrelGod | 3:6cbd7d2c78f4 | 371 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 372 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 373 | |
SquirrelGod | 3:6cbd7d2c78f4 | 374 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 375 | /* FUNCTION NAME: Preparation_prise */ |
SquirrelGod | 3:6cbd7d2c78f4 | 376 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
SquirrelGod | 3:6cbd7d2c78f4 | 377 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 378 | void Preparation_prise(unsigned char choix){ |
SquirrelGod | 2:99b1cb0d9f5e | 379 | if (choix == 1){ |
SquirrelGod | 2:99b1cb0d9f5e | 380 | multiple_myAX12->multiple_goal_and_speed(4,TAB2); |
SquirrelGod | 2:99b1cb0d9f5e | 381 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 382 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 383 | |
SquirrelGod | 3:6cbd7d2c78f4 | 384 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 385 | multiple2_myAX12->multiple_goal_and_speed(4,TAB22); |
SquirrelGod | 3:6cbd7d2c78f4 | 386 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 387 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 388 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 389 | |
SquirrelGod | 3:6cbd7d2c78f4 | 390 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 391 | /* FUNCTION NAME: Stockage_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 392 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
SquirrelGod | 3:6cbd7d2c78f4 | 393 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 394 | void Stockage_haut(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 395 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 396 | multiple_myAX12->multiple_goal_and_speed(4,TAB3); |
SquirrelGod | 3:6cbd7d2c78f4 | 397 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 398 | multiple_myAX12->multiple_goal_and_speed(4,TAB4); |
SquirrelGod | 3:6cbd7d2c78f4 | 399 | wait(TIME); |
SquirrelGod | 2:99b1cb0d9f5e | 400 | multiple_myAX12->multiple_goal_and_speed(4,TAB5); |
SquirrelGod | 2:99b1cb0d9f5e | 401 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 402 | multiple_myAX12->multiple_goal_and_speed(4,TAB6); |
SquirrelGod | 2:99b1cb0d9f5e | 403 | wait(TIME); |
SquirrelGod | 2:99b1cb0d9f5e | 404 | } |
mathieulegros | 0:a2a44c043932 | 405 | |
SquirrelGod | 2:99b1cb0d9f5e | 406 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 407 | multiple2_myAX12->multiple_goal_and_speed(4,TAB23); |
SquirrelGod | 3:6cbd7d2c78f4 | 408 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 409 | multiple2_myAX12->multiple_goal_and_speed(4,TAB24); |
SquirrelGod | 3:6cbd7d2c78f4 | 410 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 411 | multiple2_myAX12->multiple_goal_and_speed(4,TAB25); |
SquirrelGod | 3:6cbd7d2c78f4 | 412 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 413 | multiple2_myAX12->multiple_goal_and_speed(4,TAB26); |
SquirrelGod | 2:99b1cb0d9f5e | 414 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 415 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 416 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 417 | |
SquirrelGod | 3:6cbd7d2c78f4 | 418 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 419 | /* FUNCTION NAME: Stockage_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 420 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
SquirrelGod | 3:6cbd7d2c78f4 | 421 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 422 | void Stockage_bas(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 423 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 424 | multiple_myAX12->multiple_goal_and_speed(4,TAB3); |
SquirrelGod | 2:99b1cb0d9f5e | 425 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 426 | multiple_myAX12->multiple_goal_and_speed(4,TAB7); |
SquirrelGod | 2:99b1cb0d9f5e | 427 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 428 | } |
SquirrelGod | 2:99b1cb0d9f5e | 429 | |
SquirrelGod | 3:6cbd7d2c78f4 | 430 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 431 | multiple2_myAX12->multiple_goal_and_speed(4,TAB23); |
SquirrelGod | 3:6cbd7d2c78f4 | 432 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 433 | multiple2_myAX12->multiple_goal_and_speed(4,TAB27); |
SquirrelGod | 3:6cbd7d2c78f4 | 434 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 435 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 436 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 437 | |
SquirrelGod | 3:6cbd7d2c78f4 | 438 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 439 | /* FUNCTION NAME: Deposer */ |
SquirrelGod | 3:6cbd7d2c78f4 | 440 | /* DESCRIPTION : Fonction qui permet de déposer le module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 441 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 442 | void Deposer(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 443 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 444 | multiple_myAX12->multiple_goal_and_speed(4,TAB9); |
SquirrelGod | 3:6cbd7d2c78f4 | 445 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 446 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 447 | |
SquirrelGod | 3:6cbd7d2c78f4 | 448 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 449 | multiple2_myAX12->multiple_goal_and_speed(4,TAB29); |
SquirrelGod | 3:6cbd7d2c78f4 | 450 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 451 | } |
SquirrelGod | 2:99b1cb0d9f5e | 452 | } |
mathieulegros | 0:a2a44c043932 | 453 | |
SquirrelGod | 2:99b1cb0d9f5e | 454 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 455 | /* FUNCTION NAME: Preparation_depos_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 456 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
SquirrelGod | 2:99b1cb0d9f5e | 457 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 458 | void Preparation_depos_bas(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 459 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 460 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
SquirrelGod | 3:6cbd7d2c78f4 | 461 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 462 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 463 | |
SquirrelGod | 3:6cbd7d2c78f4 | 464 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 465 | multiple2_myAX12->multiple_goal_and_speed(4,TAB28); |
SquirrelGod | 3:6cbd7d2c78f4 | 466 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 467 | } |
SquirrelGod | 2:99b1cb0d9f5e | 468 | } |
SquirrelGod | 2:99b1cb0d9f5e | 469 | |
SquirrelGod | 2:99b1cb0d9f5e | 470 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 471 | /* FUNCTION NAME: Preparation_depos_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 472 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
SquirrelGod | 2:99b1cb0d9f5e | 473 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 474 | void Preparation_depos_haut(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 475 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 476 | multiple_myAX12->multiple_goal_and_speed(4,TAB6); |
SquirrelGod | 3:6cbd7d2c78f4 | 477 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 478 | multiple_myAX12->multiple_goal_and_speed(4,TAB5); |
SquirrelGod | 3:6cbd7d2c78f4 | 479 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 480 | multiple_myAX12->multiple_goal_and_speed(4,TAB10); |
SquirrelGod | 3:6cbd7d2c78f4 | 481 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 482 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
SquirrelGod | 3:6cbd7d2c78f4 | 483 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 484 | } |
mathieulegros | 0:a2a44c043932 | 485 | |
SquirrelGod | 3:6cbd7d2c78f4 | 486 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 487 | multiple2_myAX12->multiple_goal_and_speed(4,TAB26); |
SquirrelGod | 3:6cbd7d2c78f4 | 488 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 489 | multiple2_myAX12->multiple_goal_and_speed(4,TAB25); |
SquirrelGod | 3:6cbd7d2c78f4 | 490 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 491 | multiple2_myAX12->multiple_goal_and_speed(4,TAB30); |
SquirrelGod | 3:6cbd7d2c78f4 | 492 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 493 | multiple2_myAX12->multiple_goal_and_speed(4,TAB28); |
SquirrelGod | 3:6cbd7d2c78f4 | 494 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 495 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 496 | } |
SquirrelGod | 2:99b1cb0d9f5e | 497 | |
SquirrelGod | 3:6cbd7d2c78f4 | 498 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 499 | /* FUNCTION NAME: Pousser_module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 500 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 501 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 502 | void Pousser_module(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 503 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 504 | multiple_myAX12->multiple_goal_and_speed(4,TAB11); |
SquirrelGod | 3:6cbd7d2c78f4 | 505 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 506 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 507 | |
SquirrelGod | 3:6cbd7d2c78f4 | 508 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 509 | multiple2_myAX12->multiple_goal_and_speed(4,TAB31); |
SquirrelGod | 3:6cbd7d2c78f4 | 510 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 511 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 512 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 513 | |
SquirrelGod | 3:6cbd7d2c78f4 | 514 | |
SquirrelGod | 3:6cbd7d2c78f4 | 515 |