CRAC Team / command_AX12_gros_robot_v2

AX12 gros robot

Fork of command_AX12_gros_robot by CRAC Team

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?

UserRevisionLine numberNew 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