AX12 control pince
Fork of command_AX12 by
main.cpp@5:740eb0f606c1, 2017-05-11 (annotated)
- Committer:
- SquirrelGod
- Date:
- Thu May 11 12:59:23 2017 +0000
- Revision:
- 5:740eb0f606c1
- Parent:
- 4:92d6a49cd2c5
code AX12 gros robots
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:92d6a49cd2c5 | 6 | #define AX12_INITIALISATION 0 |
SquirrelGod | 4:92d6a49cd2c5 | 7 | #define AX12_PREPARATION_PRISE 1 |
SquirrelGod | 4:92d6a49cd2c5 | 8 | #define AX12_STOCKAGE_HAUT 2 |
SquirrelGod | 4:92d6a49cd2c5 | 9 | #define AX12_STOCKAGE_BAS 3 |
SquirrelGod | 4:92d6a49cd2c5 | 10 | #define AX12_DEPOSER 4 |
SquirrelGod | 4:92d6a49cd2c5 | 11 | #define AX12_PREPARATION_DEPOT_BAS 5 |
SquirrelGod | 4:92d6a49cd2c5 | 12 | #define AX12_PREPARATION_DEPOT_HAUT 6 |
SquirrelGod | 4:92d6a49cd2c5 | 13 | #define AX12_POUSSER_MODULE 7 |
SquirrelGod | 4:92d6a49cd2c5 | 14 | #define AX12_DEFAUT 20 |
SquirrelGod | 4:92d6a49cd2c5 | 15 | |
SquirrelGod | 2:99b1cb0d9f5e | 16 | #define TIME 0.8 |
SquirrelGod | 4:92d6a49cd2c5 | 17 | #define TOLERANCE_AX12 50 |
SquirrelGod | 4:92d6a49cd2c5 | 18 | #define SIZE_FIFO 25 |
SquirrelGod | 2:99b1cb0d9f5e | 19 | |
mathieulegros | 0:a2a44c043932 | 20 | /* DECLARATION VARIABLES */ |
SquirrelGod | 2:99b1cb0d9f5e | 21 | CAN can1(p30,p29); |
SquirrelGod | 2:99b1cb0d9f5e | 22 | CANMessage msgRxBuffer[SIZE_FIFO]; |
SquirrelGod | 2:99b1cb0d9f5e | 23 | unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN |
SquirrelGod | 2:99b1cb0d9f5e | 24 | unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN |
SquirrelGod | 2:99b1cb0d9f5e | 25 | |
SquirrelGod | 2:99b1cb0d9f5e | 26 | extern "C" void mbed_reset();//Pour pouvoir reset la carte |
SquirrelGod | 2:99b1cb0d9f5e | 27 | |
SquirrelGod | 4:92d6a49cd2c5 | 28 | unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 29 | static float TAB_ANGLE1[4], TAB_ANGLE2[4]; |
SquirrelGod | 4:92d6a49cd2c5 | 30 | static char TAB_POSITION[4]; |
SquirrelGod | 2:99b1cb0d9f5e | 31 | short vitesse=700; |
SquirrelGod | 2:99b1cb0d9f5e | 32 | float angle=0.0; |
SquirrelGod | 2:99b1cb0d9f5e | 33 | 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 | 34 | 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 | 35 | |
SquirrelGod | 2:99b1cb0d9f5e | 36 | |
SquirrelGod | 2:99b1cb0d9f5e | 37 | /* PROTOTYPES DE FONCTIONS ET POINTEURS */ |
SquirrelGod | 2:99b1cb0d9f5e | 38 | |
SquirrelGod | 4:92d6a49cd2c5 | 39 | |
SquirrelGod | 4:92d6a49cd2c5 | 40 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 41 | /* FUNCTION NAME: CAN2_wrFilter */ |
SquirrelGod | 4:92d6a49cd2c5 | 42 | /* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */ |
SquirrelGod | 4:92d6a49cd2c5 | 43 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 44 | void CAN2_wrFilter (uint32_t id); |
SquirrelGod | 4:92d6a49cd2c5 | 45 | |
SquirrelGod | 2:99b1cb0d9f5e | 46 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 47 | /* FUNCTION NAME: canProcessRx */ |
SquirrelGod | 2:99b1cb0d9f5e | 48 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 49 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 50 | void canProcessRx(void); |
SquirrelGod | 2:99b1cb0d9f5e | 51 | |
SquirrelGod | 2:99b1cb0d9f5e | 52 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 53 | /* FUNCTION NAME: canRx_ISR */ |
SquirrelGod | 2:99b1cb0d9f5e | 54 | /* DESCRIPTION : Interruption en réception sur le CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 55 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 56 | void canRx_ISR (void); |
SquirrelGod | 2:99b1cb0d9f5e | 57 | |
SquirrelGod | 2:99b1cb0d9f5e | 58 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 59 | /* FUNCTION NAME: SendRawId */ |
SquirrelGod | 2:99b1cb0d9f5e | 60 | /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ |
SquirrelGod | 2:99b1cb0d9f5e | 61 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 62 | void SendRawId (unsigned short id); |
SquirrelGod | 2:99b1cb0d9f5e | 63 | |
SquirrelGod | 2:99b1cb0d9f5e | 64 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 65 | /* FUNCTION NAME: Fin_action */ |
SquirrelGod | 4:92d6a49cd2c5 | 66 | /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 67 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 68 | void Fin_action(void); |
SquirrelGod | 4:92d6a49cd2c5 | 69 | |
SquirrelGod | 4:92d6a49cd2c5 | 70 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 71 | /* FUNCTION NAME: Automate_ax12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 72 | /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 73 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 74 | void AX12_automate(void); |
SquirrelGod | 4:92d6a49cd2c5 | 75 | |
SquirrelGod | 4:92d6a49cd2c5 | 76 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 77 | /* FUNCTION NAME: Initialisation_position */ |
SquirrelGod | 3:6cbd7d2c78f4 | 78 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
SquirrelGod | 3:6cbd7d2c78f4 | 79 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 80 | void Initialisation_position(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 81 | |
SquirrelGod | 3:6cbd7d2c78f4 | 82 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 83 | /* FUNCTION NAME: Preparation_prise */ |
SquirrelGod | 3:6cbd7d2c78f4 | 84 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
SquirrelGod | 3:6cbd7d2c78f4 | 85 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 86 | void Preparation_prise(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 87 | |
SquirrelGod | 3:6cbd7d2c78f4 | 88 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 89 | /* FUNCTION NAME: Stockage_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 90 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
SquirrelGod | 3:6cbd7d2c78f4 | 91 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 92 | void Stockage_haut(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 93 | |
SquirrelGod | 2:99b1cb0d9f5e | 94 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 95 | /* FUNCTION NAME: Stockage_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 96 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
SquirrelGod | 2:99b1cb0d9f5e | 97 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 98 | void Stockage_bas(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 99 | |
SquirrelGod | 3:6cbd7d2c78f4 | 100 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 101 | /* FUNCTION NAME: Deposer */ |
SquirrelGod | 3:6cbd7d2c78f4 | 102 | /* DESCRIPTION : Fonction qui permet de déposer un module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 103 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 104 | void Deposer(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 105 | |
SquirrelGod | 2:99b1cb0d9f5e | 106 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 107 | /* FUNCTION NAME: Preparation_depos_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 108 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
SquirrelGod | 2:99b1cb0d9f5e | 109 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 110 | void Preparation_depot_bas(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 111 | |
SquirrelGod | 2:99b1cb0d9f5e | 112 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 113 | /* FUNCTION NAME: Preparation_depos_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 114 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
SquirrelGod | 2:99b1cb0d9f5e | 115 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 116 | void Preparation_depot_haut(unsigned char choix); |
SquirrelGod | 3:6cbd7d2c78f4 | 117 | |
SquirrelGod | 3:6cbd7d2c78f4 | 118 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 119 | /* FUNCTION NAME: Pousser_module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 120 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 121 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 122 | void Pousser_module(unsigned char choix); |
SquirrelGod | 2:99b1cb0d9f5e | 123 | |
SquirrelGod | 4:92d6a49cd2c5 | 124 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 125 | /* FUNCTION NAME: Check_positionAX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 126 | /* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 127 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 128 | void Check_positionAX12(char* TAB, unsigned char choix); |
SquirrelGod | 4:92d6a49cd2c5 | 129 | |
mathieulegros | 0:a2a44c043932 | 130 | Timer t; |
mathieulegros | 0:a2a44c043932 | 131 | Ticker flipper; |
SquirrelGod | 2:99b1cb0d9f5e | 132 | |
SquirrelGod | 4:92d6a49cd2c5 | 133 | ///////////TABLEAU PINCE DROITE /////////////////////////// --> choix_bras == 1 |
SquirrelGod | 3:6cbd7d2c78f4 | 134 | static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale |
SquirrelGod | 3:6cbd7d2c78f4 | 135 | 0x04,0xF4, 0x01, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 136 | 0x07,0x00, 0x01, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 137 | 0x0F,0x00, 0x03, 0xFF, 0x00}; |
ConstantMBED | 1:b3ff77670606 | 138 | |
SquirrelGod | 3:6cbd7d2c78f4 | 139 | static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise |
SquirrelGod | 3:6cbd7d2c78f4 | 140 | 0x04,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 141 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 142 | 0x0F,0xF4, 0x01, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 143 | |
SquirrelGod | 3:6cbd7d2c78f4 | 144 | static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) |
SquirrelGod | 3:6cbd7d2c78f4 | 145 | 0x04,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 146 | 0x07,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 147 | 0x0F,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 2:99b1cb0d9f5e | 148 | |
SquirrelGod | 3:6cbd7d2c78f4 | 149 | static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) |
SquirrelGod | 3:6cbd7d2c78f4 | 150 | 0x04,0x00, 0x02, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 151 | 0x07,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 152 | 0x0F,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 153 | |
SquirrelGod | 3:6cbd7d2c78f4 | 154 | static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) |
SquirrelGod | 3:6cbd7d2c78f4 | 155 | 0x04,0x60, 0x01, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 156 | 0x07,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 157 | 0x0F,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 158 | |
SquirrelGod | 3:6cbd7d2c78f4 | 159 | static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 160 | 0x04,0x50, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 161 | 0x07,0x0A, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 162 | 0x0F,0xDD, 0x02, 0xFF, 0x03}; |
SquirrelGod | 3:6cbd7d2c78f4 | 163 | |
SquirrelGod | 3:6cbd7d2c78f4 | 164 | static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) |
SquirrelGod | 4:92d6a49cd2c5 | 165 | 0x04,0x50, 0x03, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 166 | 0x07,0xD5, 0x00, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 167 | 0x0F,0x4D, 0x03, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 168 | |
SquirrelGod | 4:92d6a49cd2c5 | 169 | static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer (descendre bras) |
SquirrelGod | 3:6cbd7d2c78f4 | 170 | 0x04,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 171 | 0x07,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 172 | 0x0F,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 173 | |
SquirrelGod | 4:92d6a49cd2c5 | 174 | static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 175 | 0x04,0x00, 0x03, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 176 | 0x07,0x0A, 0x01, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 177 | 0x0F,0xDD, 0x02, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 178 | |
SquirrelGod | 4:92d6a49cd2c5 | 179 | static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer (preparation module haut) |
SquirrelGod | 3:6cbd7d2c78f4 | 180 | 0x04,0x00, 0x02, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 181 | 0x07,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 182 | 0x0F,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 183 | |
SquirrelGod | 3:6cbd7d2c78f4 | 184 | static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) |
SquirrelGod | 3:6cbd7d2c78f4 | 185 | 0x04,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 186 | 0x07,0x0A, 0x01, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 187 | 0x0F,0xDD, 0x02, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 188 | |
ConstantMBED | 1:b3ff77670606 | 189 | |
SquirrelGod | 4:92d6a49cd2c5 | 190 | //////////////////TABLEAU PINCE GAUCHE ///////////////////////////// --> choix_bras == 2 |
SquirrelGod | 3:6cbd7d2c78f4 | 191 | static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale |
SquirrelGod | 3:6cbd7d2c78f4 | 192 | 0x03,0xF4, 0x01, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 193 | 0x0D,0x00, 0x01, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 194 | 0x0E,0x00, 0x03, 0xFF, 0x00}; |
SquirrelGod | 2:99b1cb0d9f5e | 195 | |
SquirrelGod | 3:6cbd7d2c78f4 | 196 | static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise |
SquirrelGod | 3:6cbd7d2c78f4 | 197 | 0x03,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 198 | 0x0D,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 199 | 0x0E,0xF4, 0x01, 0xFF, 0x03}; |
ConstantMBED | 1:b3ff77670606 | 200 | |
SquirrelGod | 3:6cbd7d2c78f4 | 201 | static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) |
SquirrelGod | 3:6cbd7d2c78f4 | 202 | 0x03,0xE9, 0x00, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 203 | 0x0D,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 204 | 0x0E,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 2:99b1cb0d9f5e | 205 | |
SquirrelGod | 3:6cbd7d2c78f4 | 206 | static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) |
SquirrelGod | 3:6cbd7d2c78f4 | 207 | 0x03,0x00, 0x02, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 208 | 0x0D,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 209 | 0x0E,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 210 | |
SquirrelGod | 3:6cbd7d2c78f4 | 211 | static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) |
SquirrelGod | 3:6cbd7d2c78f4 | 212 | 0x03,0x60, 0x01, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 213 | 0x0D,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 214 | 0x0E,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 215 | |
SquirrelGod | 3:6cbd7d2c78f4 | 216 | static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 217 | 0x03,0x50, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 218 | 0x0D,0x0A, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 219 | 0x0E,0xDD, 0x02, 0xFF, 0x03}; |
SquirrelGod | 3:6cbd7d2c78f4 | 220 | |
SquirrelGod | 3:6cbd7d2c78f4 | 221 | static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) |
SquirrelGod | 4:92d6a49cd2c5 | 222 | 0x03,0x50, 0x03, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 223 | 0x0D,0xD5, 0x00, 0xFF, 0x03, |
SquirrelGod | 4:92d6a49cd2c5 | 224 | 0x0E,0x4D, 0x03, 0xFF, 0x03}; |
ConstantMBED | 1:b3ff77670606 | 225 | |
SquirrelGod | 4:92d6a49cd2c5 | 226 | static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer(descendre bras) |
SquirrelGod | 3:6cbd7d2c78f4 | 227 | 0x03,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 228 | 0x0D,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 229 | 0x0E,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 230 | |
SquirrelGod | 4:92d6a49cd2c5 | 231 | static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer(pince ouverte) |
SquirrelGod | 3:6cbd7d2c78f4 | 232 | 0x03,0x00, 0x03, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 233 | 0x0D,0xF4, 0x01, 0xFF, 0x03, |
SquirrelGod | 3:6cbd7d2c78f4 | 234 | 0x0E,0xF4, 0x01, 0xFF, 0x03}; |
SquirrelGod | 2:99b1cb0d9f5e | 235 | |
SquirrelGod | 4:92d6a49cd2c5 | 236 | static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer(preparation module haut) |
SquirrelGod | 3:6cbd7d2c78f4 | 237 | 0x03,0x00, 0x02, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 238 | 0x0D,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 239 | 0x0E,0x4D, 0x03, 0xFF, 0x00}; |
SquirrelGod | 3:6cbd7d2c78f4 | 240 | |
SquirrelGod | 3:6cbd7d2c78f4 | 241 | static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) |
SquirrelGod | 3:6cbd7d2c78f4 | 242 | 0x03,0x00, 0x03, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 243 | 0x0D,0xD5, 0x00, 0xFF, 0x00, |
SquirrelGod | 4:92d6a49cd2c5 | 244 | 0x0E,0x4D, 0x03, 0xFF, 0x00}; |
ConstantMBED | 1:b3ff77670606 | 245 | |
SquirrelGod | 2:99b1cb0d9f5e | 246 | |
mathieulegros | 0:a2a44c043932 | 247 | /* ANGLE */ |
mathieulegros | 0:a2a44c043932 | 248 | |
mathieulegros | 0:a2a44c043932 | 249 | /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 |
mathieulegros | 0:a2a44c043932 | 250 | 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 |
mathieulegros | 0:a2a44c043932 | 251 | 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 |
mathieulegros | 0:a2a44c043932 | 252 | 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 |
mathieulegros | 0:a2a44c043932 | 253 | 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 |
mathieulegros | 0:a2a44c043932 | 254 | 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 |
mathieulegros | 0:a2a44c043932 | 255 | 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 |
mathieulegros | 0:a2a44c043932 | 256 | 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 |
mathieulegros | 0:a2a44c043932 | 257 | 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 |
mathieulegros | 0:a2a44c043932 | 258 | 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ |
mathieulegros | 0:a2a44c043932 | 259 | |
ConstantMBED | 1:b3ff77670606 | 260 | /* NUMERO AX12 */ |
ConstantMBED | 1:b3ff77670606 | 261 | |
ConstantMBED | 1:b3ff77670606 | 262 | /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 |
ConstantMBED | 1:b3ff77670606 | 263 | 1 = 0x01 | 10 = 0x0A |
ConstantMBED | 1:b3ff77670606 | 264 | 2 = 0x02 | 11 = 0x0B |
ConstantMBED | 1:b3ff77670606 | 265 | 3 = 0x03 | 12 = 0x0C |
ConstantMBED | 1:b3ff77670606 | 266 | 4 = 0x04 | 13 = 0x0D |
ConstantMBED | 1:b3ff77670606 | 267 | 5 = 0x05 | 14 = 0x0E |
ConstantMBED | 1:b3ff77670606 | 268 | 6 = 0x06 | 15 = 0x0F |
ConstantMBED | 1:b3ff77670606 | 269 | 7 = 0x07 | 16 = 0x10 |
ConstantMBED | 1:b3ff77670606 | 270 | 8 = 0x08 | 17 = 0x11 */ |
mathieulegros | 0:a2a44c043932 | 271 | |
mathieulegros | 0:a2a44c043932 | 272 | |
SquirrelGod | 2:99b1cb0d9f5e | 273 | |
SquirrelGod | 2:99b1cb0d9f5e | 274 | /* MAIN */ |
mathieulegros | 0:a2a44c043932 | 275 | |
SquirrelGod | 2:99b1cb0d9f5e | 276 | int main() |
mathieulegros | 0:a2a44c043932 | 277 | { |
SquirrelGod | 2:99b1cb0d9f5e | 278 | can1.frequency(1000000); // fréquence de travail 1Mbit/s |
SquirrelGod | 2:99b1cb0d9f5e | 279 | can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN |
SquirrelGod | 4:92d6a49cd2c5 | 280 | CAN2_wrFilter(SERVO_AX12_ACTION); |
SquirrelGod | 4:92d6a49cd2c5 | 281 | CAN2_wrFilter(SERVO_AX12_ACK); |
SquirrelGod | 4:92d6a49cd2c5 | 282 | CAN2_wrFilter(SERVO_AX12_END); |
SquirrelGod | 4:92d6a49cd2c5 | 283 | CAN2_wrFilter(CHECK_AX12); |
SquirrelGod | 2:99b1cb0d9f5e | 284 | |
SquirrelGod | 3:6cbd7d2c78f4 | 285 | trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche |
SquirrelGod | 3:6cbd7d2c78f4 | 286 | treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche |
SquirrelGod | 3:6cbd7d2c78f4 | 287 | quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche |
SquirrelGod | 3:6cbd7d2c78f4 | 288 | huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche |
SquirrelGod | 2:99b1cb0d9f5e | 289 | |
SquirrelGod | 3:6cbd7d2c78f4 | 290 | quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 291 | sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 292 | quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 293 | dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite |
SquirrelGod | 3:6cbd7d2c78f4 | 294 | |
SquirrelGod | 4:92d6a49cd2c5 | 295 | multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); //pince droite |
SquirrelGod | 4:92d6a49cd2c5 | 296 | multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); //pince gauche |
SquirrelGod | 2:99b1cb0d9f5e | 297 | |
SquirrelGod | 2:99b1cb0d9f5e | 298 | while(true) { |
SquirrelGod | 4:92d6a49cd2c5 | 299 | AX12_automate(); |
SquirrelGod | 2:99b1cb0d9f5e | 300 | canProcessRx();//Traitement des trames CAN en attente |
SquirrelGod | 2:99b1cb0d9f5e | 301 | } |
SquirrelGod | 2:99b1cb0d9f5e | 302 | } |
SquirrelGod | 2:99b1cb0d9f5e | 303 | |
SquirrelGod | 2:99b1cb0d9f5e | 304 | /* FONCTIONS */ |
SquirrelGod | 2:99b1cb0d9f5e | 305 | |
SquirrelGod | 2:99b1cb0d9f5e | 306 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 307 | /* FUNCTION NAME: canProcessRx */ |
SquirrelGod | 2:99b1cb0d9f5e | 308 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 309 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 310 | void canProcessRx(void) |
SquirrelGod | 2:99b1cb0d9f5e | 311 | { |
SquirrelGod | 2:99b1cb0d9f5e | 312 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
SquirrelGod | 4:92d6a49cd2c5 | 313 | |
SquirrelGod | 2:99b1cb0d9f5e | 314 | CANMessage msgTx=CANMessage(); |
SquirrelGod | 4:92d6a49cd2c5 | 315 | msgTx.format=CANStandard; |
SquirrelGod | 4:92d6a49cd2c5 | 316 | msgTx.type=CANData; |
SquirrelGod | 2:99b1cb0d9f5e | 317 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
SquirrelGod | 4:92d6a49cd2c5 | 318 | |
SquirrelGod | 2:99b1cb0d9f5e | 319 | if(FIFO_occupation<0) |
SquirrelGod | 2:99b1cb0d9f5e | 320 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
SquirrelGod | 4:92d6a49cd2c5 | 321 | |
SquirrelGod | 2:99b1cb0d9f5e | 322 | if(FIFO_max_occupation<FIFO_occupation) |
SquirrelGod | 2:99b1cb0d9f5e | 323 | FIFO_max_occupation=FIFO_occupation; |
SquirrelGod | 4:92d6a49cd2c5 | 324 | |
SquirrelGod | 2:99b1cb0d9f5e | 325 | if(FIFO_occupation!=0) { |
SquirrelGod | 2:99b1cb0d9f5e | 326 | |
SquirrelGod | 2:99b1cb0d9f5e | 327 | switch(msgRxBuffer[FIFO_lecture].id) { |
SquirrelGod | 4:92d6a49cd2c5 | 328 | case CHECK_AX12: |
SquirrelGod | 4:92d6a49cd2c5 | 329 | SendRawId(ALIVE_AX12); |
SquirrelGod | 4:92d6a49cd2c5 | 330 | flag = 1; |
SquirrelGod | 2:99b1cb0d9f5e | 331 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 332 | |
SquirrelGod | 4:92d6a49cd2c5 | 333 | case SERVO_AX12_ACTION : |
SquirrelGod | 4:92d6a49cd2c5 | 334 | etat_ax12 = msgRxBuffer[FIFO_lecture].data[0]; |
SquirrelGod | 4:92d6a49cd2c5 | 335 | choix_bras = msgRxBuffer[FIFO_lecture].data[1]; |
SquirrelGod | 2:99b1cb0d9f5e | 336 | |
SquirrelGod | 4:92d6a49cd2c5 | 337 | //ACK de reception des actions a effectuer |
SquirrelGod | 4:92d6a49cd2c5 | 338 | msgTx.id = SERVO_AX12_ACK; |
SquirrelGod | 4:92d6a49cd2c5 | 339 | msgTx.len = 1; |
SquirrelGod | 4:92d6a49cd2c5 | 340 | msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; |
SquirrelGod | 4:92d6a49cd2c5 | 341 | can1.write(msgTx); |
SquirrelGod | 2:99b1cb0d9f5e | 342 | break; |
SquirrelGod | 2:99b1cb0d9f5e | 343 | } |
SquirrelGod | 2:99b1cb0d9f5e | 344 | |
SquirrelGod | 2:99b1cb0d9f5e | 345 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
SquirrelGod | 2:99b1cb0d9f5e | 346 | } |
SquirrelGod | 2:99b1cb0d9f5e | 347 | } |
SquirrelGod | 2:99b1cb0d9f5e | 348 | |
SquirrelGod | 2:99b1cb0d9f5e | 349 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 350 | /* FUNCTION NAME: canRx_ISR */ |
SquirrelGod | 2:99b1cb0d9f5e | 351 | /* DESCRIPTION : Interruption en réception sur le CAN */ |
SquirrelGod | 2:99b1cb0d9f5e | 352 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 353 | void canRx_ISR (void) |
SquirrelGod | 2:99b1cb0d9f5e | 354 | { |
SquirrelGod | 2:99b1cb0d9f5e | 355 | if (can1.read(msgRxBuffer[FIFO_ecriture])) { |
SquirrelGod | 2:99b1cb0d9f5e | 356 | if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); |
SquirrelGod | 2:99b1cb0d9f5e | 357 | else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; |
SquirrelGod | 2:99b1cb0d9f5e | 358 | } |
SquirrelGod | 2:99b1cb0d9f5e | 359 | } |
SquirrelGod | 2:99b1cb0d9f5e | 360 | |
SquirrelGod | 2:99b1cb0d9f5e | 361 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 362 | /* FUNCTION NAME: SendRawId */ |
SquirrelGod | 2:99b1cb0d9f5e | 363 | /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ |
SquirrelGod | 2:99b1cb0d9f5e | 364 | /****************************************************************************************/ |
SquirrelGod | 2:99b1cb0d9f5e | 365 | void SendRawId (unsigned short id) |
SquirrelGod | 2:99b1cb0d9f5e | 366 | { |
SquirrelGod | 2:99b1cb0d9f5e | 367 | CANMessage msgTx=CANMessage(); |
SquirrelGod | 2:99b1cb0d9f5e | 368 | msgTx.id=id; |
SquirrelGod | 2:99b1cb0d9f5e | 369 | msgTx.len=0; |
SquirrelGod | 2:99b1cb0d9f5e | 370 | can1.write(msgTx); |
SquirrelGod | 2:99b1cb0d9f5e | 371 | } |
SquirrelGod | 2:99b1cb0d9f5e | 372 | void Init_AX12() // Initialisation des différents paramétres |
SquirrelGod | 2:99b1cb0d9f5e | 373 | { |
SquirrelGod | 2:99b1cb0d9f5e | 374 | deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) |
SquirrelGod | 2:99b1cb0d9f5e | 375 | quatre_myAX12-> Set_Goal_speed(vitesse); |
ConstantMBED | 1:b3ff77670606 | 376 | sept_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 377 | quatorze_myAX12-> Set_Goal_speed(vitesse); |
ConstantMBED | 1:b3ff77670606 | 378 | quinze_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 379 | dixhuit_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 380 | huit_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 381 | trois_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 382 | quatorze_myAX12-> Set_Goal_speed(vitesse); |
SquirrelGod | 2:99b1cb0d9f5e | 383 | treize_myAX12-> Set_Goal_speed(vitesse); |
mathieulegros | 0:a2a44c043932 | 384 | |
SquirrelGod | 2:99b1cb0d9f5e | 385 | deux_myAX12-> Set_Mode(0); |
ConstantMBED | 1:b3ff77670606 | 386 | quatre_myAX12-> Set_Mode(0); |
ConstantMBED | 1:b3ff77670606 | 387 | sept_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 388 | quatorze_myAX12-> Set_Mode(0); |
ConstantMBED | 1:b3ff77670606 | 389 | quinze_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 390 | dixhuit_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 391 | huit_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 392 | trois_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 393 | quatorze_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 394 | treize_myAX12-> Set_Mode(0); |
SquirrelGod | 2:99b1cb0d9f5e | 395 | } |
SquirrelGod | 2:99b1cb0d9f5e | 396 | |
SquirrelGod | 3:6cbd7d2c78f4 | 397 | |
SquirrelGod | 2:99b1cb0d9f5e | 398 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 399 | /* FUNCTION NAME: Initialisation_position */ |
SquirrelGod | 3:6cbd7d2c78f4 | 400 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
SquirrelGod | 3:6cbd7d2c78f4 | 401 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 402 | void Initialisation_position(unsigned char choix){ |
SquirrelGod | 4:92d6a49cd2c5 | 403 | if (choix == 1){ |
SquirrelGod | 4:92d6a49cd2c5 | 404 | multiple_myAX12->multiple_goal_and_speed(4,TAB1); |
SquirrelGod | 4:92d6a49cd2c5 | 405 | wait(TIME); |
SquirrelGod | 4:92d6a49cd2c5 | 406 | } |
SquirrelGod | 4:92d6a49cd2c5 | 407 | |
SquirrelGod | 4:92d6a49cd2c5 | 408 | else if (choix == 2){ |
SquirrelGod | 4:92d6a49cd2c5 | 409 | multiple2_myAX12->multiple_goal_and_speed(4,TAB21); |
SquirrelGod | 4:92d6a49cd2c5 | 410 | wait(TIME); |
SquirrelGod | 4:92d6a49cd2c5 | 411 | } |
SquirrelGod | 4:92d6a49cd2c5 | 412 | |
SquirrelGod | 3:6cbd7d2c78f4 | 413 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 414 | |
SquirrelGod | 3:6cbd7d2c78f4 | 415 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 416 | /* FUNCTION NAME: Preparation_prise */ |
SquirrelGod | 3:6cbd7d2c78f4 | 417 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
SquirrelGod | 3:6cbd7d2c78f4 | 418 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 419 | void Preparation_prise(unsigned char choix){ |
SquirrelGod | 2:99b1cb0d9f5e | 420 | if (choix == 1){ |
SquirrelGod | 2:99b1cb0d9f5e | 421 | multiple_myAX12->multiple_goal_and_speed(4,TAB2); |
SquirrelGod | 2:99b1cb0d9f5e | 422 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 423 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 424 | |
SquirrelGod | 3:6cbd7d2c78f4 | 425 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 426 | multiple2_myAX12->multiple_goal_and_speed(4,TAB22); |
SquirrelGod | 3:6cbd7d2c78f4 | 427 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 428 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 429 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 430 | |
SquirrelGod | 3:6cbd7d2c78f4 | 431 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 432 | /* FUNCTION NAME: Stockage_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 433 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
SquirrelGod | 3:6cbd7d2c78f4 | 434 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 435 | void Stockage_haut(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 436 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 437 | multiple_myAX12->multiple_goal_and_speed(4,TAB3); |
SquirrelGod | 3:6cbd7d2c78f4 | 438 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 439 | multiple_myAX12->multiple_goal_and_speed(4,TAB4); |
SquirrelGod | 3:6cbd7d2c78f4 | 440 | wait(TIME); |
SquirrelGod | 2:99b1cb0d9f5e | 441 | multiple_myAX12->multiple_goal_and_speed(4,TAB5); |
SquirrelGod | 2:99b1cb0d9f5e | 442 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 443 | multiple_myAX12->multiple_goal_and_speed(4,TAB6); |
SquirrelGod | 2:99b1cb0d9f5e | 444 | wait(TIME); |
SquirrelGod | 2:99b1cb0d9f5e | 445 | } |
mathieulegros | 0:a2a44c043932 | 446 | |
SquirrelGod | 2:99b1cb0d9f5e | 447 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 448 | multiple2_myAX12->multiple_goal_and_speed(4,TAB23); |
SquirrelGod | 3:6cbd7d2c78f4 | 449 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 450 | multiple2_myAX12->multiple_goal_and_speed(4,TAB24); |
SquirrelGod | 3:6cbd7d2c78f4 | 451 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 452 | multiple2_myAX12->multiple_goal_and_speed(4,TAB25); |
SquirrelGod | 3:6cbd7d2c78f4 | 453 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 454 | multiple2_myAX12->multiple_goal_and_speed(4,TAB26); |
SquirrelGod | 2:99b1cb0d9f5e | 455 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 456 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 457 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 458 | |
SquirrelGod | 3:6cbd7d2c78f4 | 459 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 460 | /* FUNCTION NAME: Stockage_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 461 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
SquirrelGod | 3:6cbd7d2c78f4 | 462 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 463 | void Stockage_bas(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 464 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 465 | multiple_myAX12->multiple_goal_and_speed(4,TAB3); |
SquirrelGod | 2:99b1cb0d9f5e | 466 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 467 | multiple_myAX12->multiple_goal_and_speed(4,TAB7); |
SquirrelGod | 2:99b1cb0d9f5e | 468 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 469 | } |
SquirrelGod | 2:99b1cb0d9f5e | 470 | |
SquirrelGod | 3:6cbd7d2c78f4 | 471 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 472 | multiple2_myAX12->multiple_goal_and_speed(4,TAB23); |
SquirrelGod | 3:6cbd7d2c78f4 | 473 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 474 | multiple2_myAX12->multiple_goal_and_speed(4,TAB27); |
SquirrelGod | 3:6cbd7d2c78f4 | 475 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 476 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 477 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 478 | |
SquirrelGod | 3:6cbd7d2c78f4 | 479 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 480 | /* FUNCTION NAME: Deposer */ |
SquirrelGod | 3:6cbd7d2c78f4 | 481 | /* DESCRIPTION : Fonction qui permet de déposer le module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 482 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 483 | void Deposer(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 484 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 485 | multiple_myAX12->multiple_goal_and_speed(4,TAB9); |
SquirrelGod | 3:6cbd7d2c78f4 | 486 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 487 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 488 | |
SquirrelGod | 3:6cbd7d2c78f4 | 489 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 490 | multiple2_myAX12->multiple_goal_and_speed(4,TAB29); |
SquirrelGod | 3:6cbd7d2c78f4 | 491 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 492 | } |
SquirrelGod | 2:99b1cb0d9f5e | 493 | } |
mathieulegros | 0:a2a44c043932 | 494 | |
SquirrelGod | 2:99b1cb0d9f5e | 495 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 496 | /* FUNCTION NAME: Preparation_depot_bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 497 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
SquirrelGod | 2:99b1cb0d9f5e | 498 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 499 | void Preparation_depot_bas(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 500 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 501 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
SquirrelGod | 3:6cbd7d2c78f4 | 502 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 503 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 504 | |
SquirrelGod | 3:6cbd7d2c78f4 | 505 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 506 | multiple2_myAX12->multiple_goal_and_speed(4,TAB28); |
SquirrelGod | 3:6cbd7d2c78f4 | 507 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 508 | } |
SquirrelGod | 2:99b1cb0d9f5e | 509 | } |
SquirrelGod | 2:99b1cb0d9f5e | 510 | |
SquirrelGod | 2:99b1cb0d9f5e | 511 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 512 | /* FUNCTION NAME: Preparation_depot_haut */ |
SquirrelGod | 3:6cbd7d2c78f4 | 513 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
SquirrelGod | 2:99b1cb0d9f5e | 514 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 515 | void Preparation_depot_haut(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 516 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 517 | multiple_myAX12->multiple_goal_and_speed(4,TAB6); |
SquirrelGod | 3:6cbd7d2c78f4 | 518 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 519 | multiple_myAX12->multiple_goal_and_speed(4,TAB5); |
SquirrelGod | 3:6cbd7d2c78f4 | 520 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 521 | multiple_myAX12->multiple_goal_and_speed(4,TAB10); |
SquirrelGod | 3:6cbd7d2c78f4 | 522 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 523 | multiple_myAX12->multiple_goal_and_speed(4,TAB8); |
SquirrelGod | 3:6cbd7d2c78f4 | 524 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 525 | } |
mathieulegros | 0:a2a44c043932 | 526 | |
SquirrelGod | 3:6cbd7d2c78f4 | 527 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 528 | multiple2_myAX12->multiple_goal_and_speed(4,TAB26); |
SquirrelGod | 3:6cbd7d2c78f4 | 529 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 530 | multiple2_myAX12->multiple_goal_and_speed(4,TAB25); |
SquirrelGod | 3:6cbd7d2c78f4 | 531 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 532 | multiple2_myAX12->multiple_goal_and_speed(4,TAB30); |
SquirrelGod | 3:6cbd7d2c78f4 | 533 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 534 | multiple2_myAX12->multiple_goal_and_speed(4,TAB28); |
SquirrelGod | 3:6cbd7d2c78f4 | 535 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 536 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 537 | } |
SquirrelGod | 2:99b1cb0d9f5e | 538 | |
SquirrelGod | 3:6cbd7d2c78f4 | 539 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 540 | /* FUNCTION NAME: Pousser_module */ |
SquirrelGod | 3:6cbd7d2c78f4 | 541 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
SquirrelGod | 3:6cbd7d2c78f4 | 542 | /****************************************************************************************/ |
SquirrelGod | 3:6cbd7d2c78f4 | 543 | void Pousser_module(unsigned char choix){ |
SquirrelGod | 3:6cbd7d2c78f4 | 544 | if (choix == 1){ |
SquirrelGod | 3:6cbd7d2c78f4 | 545 | multiple_myAX12->multiple_goal_and_speed(4,TAB11); |
SquirrelGod | 3:6cbd7d2c78f4 | 546 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 547 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 548 | |
SquirrelGod | 3:6cbd7d2c78f4 | 549 | else if (choix == 2){ |
SquirrelGod | 3:6cbd7d2c78f4 | 550 | multiple2_myAX12->multiple_goal_and_speed(4,TAB31); |
SquirrelGod | 3:6cbd7d2c78f4 | 551 | wait(TIME); |
SquirrelGod | 3:6cbd7d2c78f4 | 552 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 553 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 554 | |
SquirrelGod | 4:92d6a49cd2c5 | 555 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 556 | /* FUNCTION NAME: Fin_action */ |
SquirrelGod | 4:92d6a49cd2c5 | 557 | /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 558 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 559 | void Fin_action(void){ |
SquirrelGod | 4:92d6a49cd2c5 | 560 | CANMessage msgTx=CANMessage(); |
SquirrelGod | 4:92d6a49cd2c5 | 561 | msgTx.format=CANStandard; |
SquirrelGod | 4:92d6a49cd2c5 | 562 | msgTx.type=CANData; |
SquirrelGod | 4:92d6a49cd2c5 | 563 | |
SquirrelGod | 4:92d6a49cd2c5 | 564 | msgTx.id = SERVO_AX12_END; |
SquirrelGod | 4:92d6a49cd2c5 | 565 | msgTx.len = 1; |
SquirrelGod | 4:92d6a49cd2c5 | 566 | msgTx.data[0] = AX12_PREPARATION_PRISE; |
SquirrelGod | 4:92d6a49cd2c5 | 567 | can1.write(msgTx); |
SquirrelGod | 4:92d6a49cd2c5 | 568 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 569 | |
SquirrelGod | 4:92d6a49cd2c5 | 570 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 571 | /* FUNCTION NAME: Automate_ax12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 572 | /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 573 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 574 | void AX12_automate(void){ |
SquirrelGod | 4:92d6a49cd2c5 | 575 | switch(etat_ax12){ |
SquirrelGod | 4:92d6a49cd2c5 | 576 | case AX12_INITIALISATION : |
SquirrelGod | 4:92d6a49cd2c5 | 577 | if (flag == 1){ |
SquirrelGod | 4:92d6a49cd2c5 | 578 | Initialisation_position(1); |
SquirrelGod | 4:92d6a49cd2c5 | 579 | //Check_positionAX12(&TAB1[25], 1); |
SquirrelGod | 4:92d6a49cd2c5 | 580 | Initialisation_position(2); |
SquirrelGod | 4:92d6a49cd2c5 | 581 | //Check_positionAX12(&TAB21[25], 2); |
SquirrelGod | 4:92d6a49cd2c5 | 582 | flag = 2; |
SquirrelGod | 4:92d6a49cd2c5 | 583 | } |
SquirrelGod | 4:92d6a49cd2c5 | 584 | else if (choix_bras > 1){ |
SquirrelGod | 4:92d6a49cd2c5 | 585 | Initialisation_position(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 586 | } |
SquirrelGod | 4:92d6a49cd2c5 | 587 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 588 | |
SquirrelGod | 4:92d6a49cd2c5 | 589 | case AX12_PREPARATION_PRISE : |
SquirrelGod | 4:92d6a49cd2c5 | 590 | Preparation_prise(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 591 | if (action == 0){ |
SquirrelGod | 4:92d6a49cd2c5 | 592 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 593 | action ++; |
SquirrelGod | 4:92d6a49cd2c5 | 594 | } |
SquirrelGod | 4:92d6a49cd2c5 | 595 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 596 | |
SquirrelGod | 4:92d6a49cd2c5 | 597 | case AX12_STOCKAGE_HAUT : |
SquirrelGod | 4:92d6a49cd2c5 | 598 | Stockage_haut(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 599 | etat_ax12 = AX12_DEFAUT; |
SquirrelGod | 4:92d6a49cd2c5 | 600 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 601 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 602 | |
SquirrelGod | 4:92d6a49cd2c5 | 603 | case AX12_STOCKAGE_BAS : |
SquirrelGod | 4:92d6a49cd2c5 | 604 | Stockage_bas(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 605 | etat_ax12 = AX12_DEFAUT; |
SquirrelGod | 4:92d6a49cd2c5 | 606 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 607 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 608 | |
SquirrelGod | 4:92d6a49cd2c5 | 609 | case AX12_DEPOSER : |
SquirrelGod | 4:92d6a49cd2c5 | 610 | Deposer(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 611 | etat_ax12 = AX12_DEFAUT; |
SquirrelGod | 4:92d6a49cd2c5 | 612 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 613 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 614 | |
SquirrelGod | 4:92d6a49cd2c5 | 615 | case AX12_PREPARATION_DEPOT_BAS : |
SquirrelGod | 4:92d6a49cd2c5 | 616 | Preparation_depot_bas(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 617 | etat_ax12 = AX12_DEFAUT; |
SquirrelGod | 4:92d6a49cd2c5 | 618 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 619 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 620 | |
SquirrelGod | 4:92d6a49cd2c5 | 621 | case AX12_PREPARATION_DEPOT_HAUT : |
SquirrelGod | 4:92d6a49cd2c5 | 622 | Preparation_depot_haut(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 623 | etat_ax12 = AX12_DEFAUT; |
SquirrelGod | 4:92d6a49cd2c5 | 624 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 625 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 626 | |
SquirrelGod | 4:92d6a49cd2c5 | 627 | case AX12_POUSSER_MODULE : |
SquirrelGod | 4:92d6a49cd2c5 | 628 | Pousser_module(choix_bras); |
SquirrelGod | 4:92d6a49cd2c5 | 629 | etat_ax12 = AX12_DEFAUT; |
SquirrelGod | 4:92d6a49cd2c5 | 630 | Fin_action(); |
SquirrelGod | 4:92d6a49cd2c5 | 631 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 632 | |
SquirrelGod | 4:92d6a49cd2c5 | 633 | case AX12_DEFAUT : |
SquirrelGod | 4:92d6a49cd2c5 | 634 | action = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 635 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 636 | } |
SquirrelGod | 4:92d6a49cd2c5 | 637 | } |
SquirrelGod | 3:6cbd7d2c78f4 | 638 | |
SquirrelGod | 4:92d6a49cd2c5 | 639 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 640 | /* FUNCTION NAME: CAN2_wrFilter */ |
SquirrelGod | 4:92d6a49cd2c5 | 641 | /* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */ |
SquirrelGod | 4:92d6a49cd2c5 | 642 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 643 | void CAN2_wrFilter (uint32_t id) { |
SquirrelGod | 4:92d6a49cd2c5 | 644 | static int CAN_std_cnt = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 645 | uint32_t buf0, buf1; |
SquirrelGod | 4:92d6a49cd2c5 | 646 | int cnt1, cnt2, bound1; |
SquirrelGod | 4:92d6a49cd2c5 | 647 | |
SquirrelGod | 4:92d6a49cd2c5 | 648 | /* Acceptance Filter Memory full */ |
SquirrelGod | 4:92d6a49cd2c5 | 649 | if (((CAN_std_cnt + 1) >> 1) >= 512) |
SquirrelGod | 4:92d6a49cd2c5 | 650 | return; /* error: objects full */ |
SquirrelGod | 4:92d6a49cd2c5 | 651 | |
SquirrelGod | 4:92d6a49cd2c5 | 652 | /* Setup Acceptance Filter Configuration |
SquirrelGod | 4:92d6a49cd2c5 | 653 | Acceptance Filter Mode Register = Off */ |
SquirrelGod | 4:92d6a49cd2c5 | 654 | LPC_CANAF->AFMR = 0x00000001; |
SquirrelGod | 4:92d6a49cd2c5 | 655 | |
SquirrelGod | 4:92d6a49cd2c5 | 656 | id |= 1 << 13; /* Add controller number(2) */ |
SquirrelGod | 4:92d6a49cd2c5 | 657 | id &= 0x0000F7FF; /* Mask out 16-bits of ID */ |
SquirrelGod | 4:92d6a49cd2c5 | 658 | |
SquirrelGod | 4:92d6a49cd2c5 | 659 | if (CAN_std_cnt == 0) { /* For entering first ID */ |
SquirrelGod | 4:92d6a49cd2c5 | 660 | LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16); |
SquirrelGod | 4:92d6a49cd2c5 | 661 | } else if (CAN_std_cnt == 1) { /* For entering second ID */ |
SquirrelGod | 4:92d6a49cd2c5 | 662 | if ((LPC_CANAF_RAM->mask[0] >> 16) > id) |
SquirrelGod | 4:92d6a49cd2c5 | 663 | LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16); |
SquirrelGod | 4:92d6a49cd2c5 | 664 | else |
SquirrelGod | 4:92d6a49cd2c5 | 665 | LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id; |
SquirrelGod | 4:92d6a49cd2c5 | 666 | } else { |
SquirrelGod | 4:92d6a49cd2c5 | 667 | /* Find where to insert new ID */ |
SquirrelGod | 4:92d6a49cd2c5 | 668 | cnt1 = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 669 | cnt2 = CAN_std_cnt; |
SquirrelGod | 4:92d6a49cd2c5 | 670 | bound1 = (CAN_std_cnt - 1) >> 1; |
SquirrelGod | 4:92d6a49cd2c5 | 671 | while (cnt1 <= bound1) { /* Loop through standard existing IDs */ |
SquirrelGod | 4:92d6a49cd2c5 | 672 | if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id) { |
SquirrelGod | 4:92d6a49cd2c5 | 673 | cnt2 = cnt1 * 2; |
SquirrelGod | 4:92d6a49cd2c5 | 674 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 675 | } |
SquirrelGod | 4:92d6a49cd2c5 | 676 | if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id) { |
SquirrelGod | 4:92d6a49cd2c5 | 677 | cnt2 = cnt1 * 2 + 1; |
SquirrelGod | 4:92d6a49cd2c5 | 678 | break; |
SquirrelGod | 4:92d6a49cd2c5 | 679 | } |
SquirrelGod | 4:92d6a49cd2c5 | 680 | cnt1++; /* cnt1 = U32 where to insert new ID */ |
SquirrelGod | 4:92d6a49cd2c5 | 681 | } /* cnt2 = U16 where to insert new ID */ |
SquirrelGod | 4:92d6a49cd2c5 | 682 | |
SquirrelGod | 4:92d6a49cd2c5 | 683 | if (cnt1 > bound1) { /* Adding ID as last entry */ |
SquirrelGod | 4:92d6a49cd2c5 | 684 | if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */ |
SquirrelGod | 4:92d6a49cd2c5 | 685 | LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16); |
SquirrelGod | 4:92d6a49cd2c5 | 686 | else /* Odd number of IDs exists */ |
SquirrelGod | 4:92d6a49cd2c5 | 687 | LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id; |
SquirrelGod | 4:92d6a49cd2c5 | 688 | } else { |
SquirrelGod | 4:92d6a49cd2c5 | 689 | buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */ |
SquirrelGod | 4:92d6a49cd2c5 | 690 | if ((cnt2 & 0x0001) == 0) /* Insert new mask to even address */ |
SquirrelGod | 4:92d6a49cd2c5 | 691 | buf1 = (id << 16) | (buf0 >> 16); |
SquirrelGod | 4:92d6a49cd2c5 | 692 | else /* Insert new mask to odd address */ |
SquirrelGod | 4:92d6a49cd2c5 | 693 | buf1 = (buf0 & 0xFFFF0000) | id; |
SquirrelGod | 4:92d6a49cd2c5 | 694 | |
SquirrelGod | 4:92d6a49cd2c5 | 695 | LPC_CANAF_RAM->mask[cnt1] = buf1; /* Insert mask */ |
SquirrelGod | 4:92d6a49cd2c5 | 696 | |
SquirrelGod | 4:92d6a49cd2c5 | 697 | bound1 = CAN_std_cnt >> 1; |
SquirrelGod | 4:92d6a49cd2c5 | 698 | /* Move all remaining standard mask entries one place up */ |
SquirrelGod | 4:92d6a49cd2c5 | 699 | while (cnt1 < bound1) { |
SquirrelGod | 4:92d6a49cd2c5 | 700 | cnt1++; |
SquirrelGod | 4:92d6a49cd2c5 | 701 | buf1 = LPC_CANAF_RAM->mask[cnt1]; |
SquirrelGod | 4:92d6a49cd2c5 | 702 | LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16); |
SquirrelGod | 4:92d6a49cd2c5 | 703 | buf0 = buf1; |
SquirrelGod | 4:92d6a49cd2c5 | 704 | } |
SquirrelGod | 4:92d6a49cd2c5 | 705 | |
SquirrelGod | 4:92d6a49cd2c5 | 706 | if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */ |
SquirrelGod | 4:92d6a49cd2c5 | 707 | LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF); |
SquirrelGod | 4:92d6a49cd2c5 | 708 | } |
SquirrelGod | 4:92d6a49cd2c5 | 709 | } |
SquirrelGod | 4:92d6a49cd2c5 | 710 | CAN_std_cnt++; |
SquirrelGod | 4:92d6a49cd2c5 | 711 | |
SquirrelGod | 4:92d6a49cd2c5 | 712 | /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */ |
SquirrelGod | 4:92d6a49cd2c5 | 713 | buf0 = ((CAN_std_cnt + 1) >> 1) << 2; |
SquirrelGod | 4:92d6a49cd2c5 | 714 | buf1 = buf0; |
SquirrelGod | 4:92d6a49cd2c5 | 715 | |
SquirrelGod | 4:92d6a49cd2c5 | 716 | /* Setup acceptance filter pointers */ |
SquirrelGod | 4:92d6a49cd2c5 | 717 | LPC_CANAF->SFF_sa = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 718 | LPC_CANAF->SFF_GRP_sa = buf0; |
SquirrelGod | 4:92d6a49cd2c5 | 719 | LPC_CANAF->EFF_sa = buf0; |
SquirrelGod | 4:92d6a49cd2c5 | 720 | LPC_CANAF->EFF_GRP_sa = buf1; |
SquirrelGod | 4:92d6a49cd2c5 | 721 | LPC_CANAF->ENDofTable = buf1; |
SquirrelGod | 4:92d6a49cd2c5 | 722 | |
SquirrelGod | 4:92d6a49cd2c5 | 723 | LPC_CANAF->AFMR = 0x00000000; /* Use acceptance filter */ |
SquirrelGod | 4:92d6a49cd2c5 | 724 | } // CAN2_wrFilter |
SquirrelGod | 4:92d6a49cd2c5 | 725 | |
SquirrelGod | 4:92d6a49cd2c5 | 726 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 727 | /* FUNCTION NAME: Check_positionAX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 728 | /* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */ |
SquirrelGod | 4:92d6a49cd2c5 | 729 | /****************************************************************************************/ |
SquirrelGod | 4:92d6a49cd2c5 | 730 | void Check_positionAX12(char* TAB, unsigned char choix){ |
SquirrelGod | 4:92d6a49cd2c5 | 731 | int k=1, i=0; |
SquirrelGod | 4:92d6a49cd2c5 | 732 | static float TAB_POS_TH[4]; |
SquirrelGod | 4:92d6a49cd2c5 | 733 | |
SquirrelGod | 4:92d6a49cd2c5 | 734 | CANMessage msgTx=CANMessage(); |
SquirrelGod | 4:92d6a49cd2c5 | 735 | msgTx.id=SERVO_AX12_POSITION; |
SquirrelGod | 4:92d6a49cd2c5 | 736 | msgTx.len=5; |
SquirrelGod | 4:92d6a49cd2c5 | 737 | |
SquirrelGod | 4:92d6a49cd2c5 | 738 | //PERMET DE VERIFIER LA POSITION D'UN AX12 |
SquirrelGod | 4:92d6a49cd2c5 | 739 | TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 740 | TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 741 | TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 742 | TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 743 | TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 744 | TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 745 | TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 746 | TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3); |
SquirrelGod | 4:92d6a49cd2c5 | 747 | |
SquirrelGod | 4:92d6a49cd2c5 | 748 | for(i=0; i<4; i++){ |
SquirrelGod | 4:92d6a49cd2c5 | 749 | TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8); |
SquirrelGod | 4:92d6a49cd2c5 | 750 | k=k+5; |
SquirrelGod | 4:92d6a49cd2c5 | 751 | } |
SquirrelGod | 4:92d6a49cd2c5 | 752 | |
SquirrelGod | 4:92d6a49cd2c5 | 753 | for(i=0; i<4; i++){ |
SquirrelGod | 4:92d6a49cd2c5 | 754 | if (choix == 1){ |
SquirrelGod | 4:92d6a49cd2c5 | 755 | if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ |
SquirrelGod | 4:92d6a49cd2c5 | 756 | TAB_POSITION[i] = 1; |
SquirrelGod | 4:92d6a49cd2c5 | 757 | } |
SquirrelGod | 4:92d6a49cd2c5 | 758 | else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ |
SquirrelGod | 4:92d6a49cd2c5 | 759 | TAB_POSITION[i] = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 760 | } |
SquirrelGod | 4:92d6a49cd2c5 | 761 | } |
SquirrelGod | 4:92d6a49cd2c5 | 762 | else if (choix == 2){ |
SquirrelGod | 4:92d6a49cd2c5 | 763 | if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ |
SquirrelGod | 4:92d6a49cd2c5 | 764 | TAB_POSITION[i] = 1; |
SquirrelGod | 4:92d6a49cd2c5 | 765 | } |
SquirrelGod | 4:92d6a49cd2c5 | 766 | else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ |
SquirrelGod | 4:92d6a49cd2c5 | 767 | TAB_POSITION[i] = 0; |
SquirrelGod | 4:92d6a49cd2c5 | 768 | } |
SquirrelGod | 4:92d6a49cd2c5 | 769 | } |
SquirrelGod | 4:92d6a49cd2c5 | 770 | } |
SquirrelGod | 4:92d6a49cd2c5 | 771 | |
SquirrelGod | 4:92d6a49cd2c5 | 772 | msgTx.data[0] = choix; |
SquirrelGod | 4:92d6a49cd2c5 | 773 | for(i=1; i<5; i++){ |
SquirrelGod | 4:92d6a49cd2c5 | 774 | msgTx.data[i] = TAB_POSITION[i]; |
SquirrelGod | 4:92d6a49cd2c5 | 775 | } |
SquirrelGod | 4:92d6a49cd2c5 | 776 | can1.write(msgTx); |
SquirrelGod | 4:92d6a49cd2c5 | 777 | } |
SquirrelGod | 4:92d6a49cd2c5 | 778 |