CRAC Team / Utilisatio_MX12_V3

Programme d'utilisation des AX12 et de l'MX12 V3

Fork of Utilisatio_MX12_V2 by CRAC Team

Actionneur.cpp@4:0972025782f3, 2017-05-20 (annotated)

Committer:
R66Y
Date:
Sat May 20 16:16:00 2017 +0000
Revision:
4:0972025782f3
Parent:
3:1bb26049bdd1 
Programme d'utilisation des AX et de l'MX12 V3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
matthieuvignon 2:9d280856a536 1 #include "Actionneur.h"
matthieuvignon 2:9d280856a536 2
matthieuvignon 2:9d280856a536 3 extern CAN can;
matthieuvignon 2:9d280856a536 4
matthieuvignon 2:9d280856a536 5 /* DECLARATION VARIABLES */
matthieuvignon 2:9d280856a536 6
matthieuvignon 2:9d280856a536 7 extern unsigned char FlagAx12;
matthieuvignon 2:9d280856a536 8 extern "C" void mbed_reset();//Pour pouvoir reset la carte
matthieuvignon 2:9d280856a536 9
matthieuvignon 2:9d280856a536 10 extern DigitalOut led2;
matthieuvignon 2:9d280856a536 11 extern Serial pc;
matthieuvignon 2:9d280856a536 12 extern void GetPositionAx12(void);
matthieuvignon 2:9d280856a536 13 extern void gerer_turbine(unsigned char pwm_turbine);
matthieuvignon 2:9d280856a536 14 extern PwmOut PompeDroite;
matthieuvignon 2:9d280856a536 15 extern unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
matthieuvignon 2:9d280856a536 16 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
matthieuvignon 2:9d280856a536 17 unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3);
matthieuvignon 2:9d280856a536 18
matthieuvignon 2:9d280856a536 19 static float TAB_ANGLE1[4], TAB_ANGLE2[4];
matthieuvignon 2:9d280856a536 20 static char TAB_POSITION[4];
R66Y 3:1bb26049bdd1 21 AX12 *un_myMX12, *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
R66Y 3:1bb26049bdd1 22 AX12 *TrappeLanceur, *MilieuBrasPompe, *HautBrasPompe, *BasBrasPompe, *BrasPompeAx12, *TabPompeBras;
matthieuvignon 2:9d280856a536 23
matthieuvignon 2:9d280856a536 24
matthieuvignon 2:9d280856a536 25
matthieuvignon 2:9d280856a536 26 /* ANGLE */
matthieuvignon 2:9d280856a536 27
matthieuvignon 2:9d280856a536 28 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
matthieuvignon 2:9d280856a536 29 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
matthieuvignon 2:9d280856a536 30 30° = 0x64, 0x00 | 130°= 0xB1, 0x01
matthieuvignon 2:9d280856a536 31 40° = 0x85, 0x00 | 140°= 0xD2, 0x01
matthieuvignon 2:9d280856a536 32 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
matthieuvignon 2:9d280856a536 33 60° = 0xC8, 0x00 | 160°= 0x15, 0x02
matthieuvignon 2:9d280856a536 34 70° = 0xE9, 0x00 | 170°= 0x36, 0x02
matthieuvignon 2:9d280856a536 35 80° = 0x0A, 0x01 | 180°= 0x58, 0x02
matthieuvignon 2:9d280856a536 36 90° = 0x2C, 0x01 | 190°= 0x79, 0x02
matthieuvignon 2:9d280856a536 37 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
matthieuvignon 2:9d280856a536 38
matthieuvignon 2:9d280856a536 39 /* NUMERO AX12 */
matthieuvignon 2:9d280856a536 40
matthieuvignon 2:9d280856a536 41 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
matthieuvignon 2:9d280856a536 42 1 = 0x01 | 10 = 0x0A
matthieuvignon 2:9d280856a536 43 2 = 0x02 | 11 = 0x0B
matthieuvignon 2:9d280856a536 44 3 = 0x03 | 12 = 0x0C
matthieuvignon 2:9d280856a536 45 4 = 0x04 | 13 = 0x0D
matthieuvignon 2:9d280856a536 46 5 = 0x05 | 14 = 0x0E
matthieuvignon 2:9d280856a536 47 6 = 0x06 | 15 = 0x0F
matthieuvignon 2:9d280856a536 48 7 = 0x07 | 16 = 0x10
matthieuvignon 2:9d280856a536 49 8 = 0x08 | 17 = 0x11 */
matthieuvignon 2:9d280856a536 50
matthieuvignon 2:9d280856a536 51
matthieuvignon 2:9d280856a536 52
matthieuvignon 2:9d280856a536 53 /* MAIN */
matthieuvignon 2:9d280856a536 54
matthieuvignon 2:9d280856a536 55 void initialisation_AX12(void)
matthieuvignon 2:9d280856a536 56 {
matthieuvignon 2:9d280856a536 57 short vitesse=700;
matthieuvignon 2:9d280856a536 58
R66Y 3:1bb26049bdd1 59 TrappeLanceur = new AX12(p9, p10, 1, 1000000); // MX12
R66Y 3:1bb26049bdd1 60
matthieuvignon 2:9d280856a536 61 HautBrasPompe = new AX12(p9, p10, 3, 1000000);
matthieuvignon 2:9d280856a536 62 MilieuBrasPompe = new AX12(p9, p10, 2, 1000000);
matthieuvignon 2:9d280856a536 63 BasBrasPompe = new AX12(p9, p10, 1, 1000000);
matthieuvignon 2:9d280856a536 64
matthieuvignon 2:9d280856a536 65 BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
matthieuvignon 2:9d280856a536 66
R66Y 3:1bb26049bdd1 67 TrappeLanceur->Set_Goal_speed(vitesse); // MX12
R66Y 3:1bb26049bdd1 68
matthieuvignon 2:9d280856a536 69 HautBrasPompe->Set_Goal_speed(vitesse);
matthieuvignon 2:9d280856a536 70 MilieuBrasPompe->Set_Goal_speed(vitesse);
matthieuvignon 2:9d280856a536 71 BasBrasPompe->Set_Goal_speed(vitesse);
matthieuvignon 2:9d280856a536 72
R66Y 3:1bb26049bdd1 73 TrappeLanceur->Set_Mode(0); // MX12
R66Y 3:1bb26049bdd1 74
matthieuvignon 2:9d280856a536 75 HautBrasPompe->Set_Mode(0);
matthieuvignon 2:9d280856a536 76 MilieuBrasPompe->Set_Mode(0);
matthieuvignon 2:9d280856a536 77 BasBrasPompe->Set_Mode(0);
matthieuvignon 2:9d280856a536 78
matthieuvignon 2:9d280856a536 79 }
matthieuvignon 2:9d280856a536 80
matthieuvignon 2:9d280856a536 81 void GetPositionAx12(void) {
matthieuvignon 2:9d280856a536 82
matthieuvignon 2:9d280856a536 83 pc.printf("\n\r * Bras Pompe * \n\r");
matthieuvignon 2:9d280856a536 84
matthieuvignon 2:9d280856a536 85 pc.printf("Haut : %lf \n\r ", HautBrasPompe->Get_Position() );
matthieuvignon 2:9d280856a536 86 pc.printf("Milieu : %lf \n\r ", MilieuBrasPompe->Get_Position() );
R66Y 3:1bb26049bdd1 87 pc.printf("Bas: %lf \n\r ", BasBrasPompe->Get_Position() );
R66Y 3:1bb26049bdd1 88 pc.printf("Bas: %lf \n\r ", TrappeLanceur->Get_Position() ); // MX12
matthieuvignon 2:9d280856a536 89
matthieuvignon 2:9d280856a536 90 }
matthieuvignon 2:9d280856a536 91
matthieuvignon 2:9d280856a536 92
matthieuvignon 2:9d280856a536 93 /****************************************************************************************/
matthieuvignon 2:9d280856a536 94 /* FUNCTION NAME: Automate_ax12 */
matthieuvignon 2:9d280856a536 95 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
matthieuvignon 2:9d280856a536 96 /****************************************************************************************/
matthieuvignon 2:9d280856a536 97 void AX12_automate(unsigned char etat_ax12, unsigned char choix_bras){
matthieuvignon 2:9d280856a536 98
matthieuvignon 2:9d280856a536 99 unsigned short speed;
matthieuvignon 2:9d280856a536 100
matthieuvignon 2:9d280856a536 101 static unsigned char action = 0;
matthieuvignon 2:9d280856a536 102 unsigned int GoalPos1, GoalPos2, GoalPos3;
matthieuvignon 2:9d280856a536 103 switch(etat_ax12){
matthieuvignon 2:9d280856a536 104
R66Y 3:1bb26049bdd1 105 case AX12_GET_POSITION: // 10 (10)
matthieuvignon 2:9d280856a536 106 GetPositionAx12();
matthieuvignon 2:9d280856a536 107 break;
matthieuvignon 2:9d280856a536 108
matthieuvignon 2:9d280856a536 109
R66Y 3:1bb26049bdd1 110 case AX12_VIDER_FUSEE_POMPE: // 8 (10)
matthieuvignon 2:9d280856a536 111
matthieuvignon 2:9d280856a536 112 speed=511;
matthieuvignon 2:9d280856a536 113
matthieuvignon 2:9d280856a536 114 GoalPos1=1500;
matthieuvignon 2:9d280856a536 115 GoalPos2=624;
matthieuvignon 2:9d280856a536 116 GoalPos3=1413;
matthieuvignon 2:9d280856a536 117 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 118 GoalPos1+=900;
matthieuvignon 2:9d280856a536 119 GoalPos2=202;
matthieuvignon 2:9d280856a536 120 GoalPos3=2205;
matthieuvignon 2:9d280856a536 121 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(20*TIME);
matthieuvignon 2:9d280856a536 122 GoalPos3=2557;
matthieuvignon 2:9d280856a536 123 GoalPos2=173;
matthieuvignon 2:9d280856a536 124 GoalPos1=1947;
matthieuvignon 2:9d280856a536 125 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 126 GoalPos3=2557;
matthieuvignon 2:9d280856a536 127 GoalPos2=370;
matthieuvignon 2:9d280856a536 128 GoalPos1=1798;
matthieuvignon 2:9d280856a536 129 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 130 GoalPos3=2548;
matthieuvignon 2:9d280856a536 131 GoalPos2=683;
matthieuvignon 2:9d280856a536 132 GoalPos1=1499;
matthieuvignon 2:9d280856a536 133 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 134 GoalPos3=2405;
matthieuvignon 2:9d280856a536 135 GoalPos2=1150;
matthieuvignon 2:9d280856a536 136 GoalPos1=1158;
matthieuvignon 2:9d280856a536 137 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 138
matthieuvignon 2:9d280856a536 139 PompeDroite.write(0);
matthieuvignon 2:9d280856a536 140
matthieuvignon 2:9d280856a536 141 Fin_action();
matthieuvignon 2:9d280856a536 142 break;
matthieuvignon 2:9d280856a536 143
R66Y 3:1bb26049bdd1 144 case AX12_VIDER_FUSEE_POMPEBLEU: // 11 (10)
matthieuvignon 2:9d280856a536 145
matthieuvignon 2:9d280856a536 146 speed=511;
matthieuvignon 2:9d280856a536 147
matthieuvignon 2:9d280856a536 148 GoalPos3=2205;
matthieuvignon 2:9d280856a536 149 GoalPos2=202;
matthieuvignon 2:9d280856a536 150 GoalPos1=598;
matthieuvignon 2:9d280856a536 151 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 152 GoalPos3=2557;
matthieuvignon 2:9d280856a536 153 GoalPos2=173;
matthieuvignon 2:9d280856a536 154 GoalPos1=1050;
matthieuvignon 2:9d280856a536 155 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(5*TIME);
matthieuvignon 2:9d280856a536 156 GoalPos3=2557;
matthieuvignon 2:9d280856a536 157 GoalPos2=370;
matthieuvignon 2:9d280856a536 158 GoalPos1=1199;
matthieuvignon 2:9d280856a536 159 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 160 GoalPos3=2548;
matthieuvignon 2:9d280856a536 161 GoalPos2=683;
matthieuvignon 2:9d280856a536 162 GoalPos1=1499;
matthieuvignon 2:9d280856a536 163 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 164 GoalPos3=2405;
matthieuvignon 2:9d280856a536 165 GoalPos2=1150;
matthieuvignon 2:9d280856a536 166 GoalPos1=1947;
matthieuvignon 2:9d280856a536 167 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 168
matthieuvignon 2:9d280856a536 169 PompeDroite.write(0);
matthieuvignon 2:9d280856a536 170
matthieuvignon 2:9d280856a536 171 Fin_action();
matthieuvignon 2:9d280856a536 172 break;
matthieuvignon 2:9d280856a536 173
R66Y 3:1bb26049bdd1 174 case AX12_REMPLIR_BASE_POMPE: // 9 (10)
matthieuvignon 2:9d280856a536 175
matthieuvignon 2:9d280856a536 176 speed=511;
matthieuvignon 2:9d280856a536 177
matthieuvignon 2:9d280856a536 178 GoalPos3=2440;
matthieuvignon 2:9d280856a536 179 GoalPos2=302;
matthieuvignon 2:9d280856a536 180 GoalPos1=1950;
matthieuvignon 2:9d280856a536 181 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 182 GoalPos3=1865;
matthieuvignon 2:9d280856a536 183 GoalPos2=252;
matthieuvignon 2:9d280856a536 184 GoalPos1=2400;
matthieuvignon 2:9d280856a536 185 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 186 GoalPos3=1500;
matthieuvignon 2:9d280856a536 187 GoalPos2=460;
matthieuvignon 2:9d280856a536 188 GoalPos1=2400;
matthieuvignon 2:9d280856a536 189 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 190 GoalPos3=1396;
matthieuvignon 2:9d280856a536 191 GoalPos2=600;
matthieuvignon 2:9d280856a536 192 GoalPos1=1399;
matthieuvignon 2:9d280856a536 193 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 194
matthieuvignon 2:9d280856a536 195 GoalPos3=1413;
matthieuvignon 2:9d280856a536 196 GoalPos2=624;
matthieuvignon 2:9d280856a536 197 GoalPos1=1500;
matthieuvignon 2:9d280856a536 198 mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
matthieuvignon 2:9d280856a536 199
matthieuvignon 2:9d280856a536 200 Fin_action();
matthieuvignon 2:9d280856a536 201 break;
matthieuvignon 2:9d280856a536 202
R66Y 3:1bb26049bdd1 203 case AX12_INITIALISATION : // 0
matthieuvignon 2:9d280856a536 204 if (FlagAx12 == 1){
matthieuvignon 2:9d280856a536 205 Initialisation_position(1);
matthieuvignon 2:9d280856a536 206 //Check_positionAX12(&TAB1[25], 1);
matthieuvignon 2:9d280856a536 207 Initialisation_position(2);
matthieuvignon 2:9d280856a536 208 //Check_positionAX12(&TAB21[25], 2);
matthieuvignon 2:9d280856a536 209 FlagAx12 = 2;
matthieuvignon 2:9d280856a536 210 }
matthieuvignon 2:9d280856a536 211 else if (choix_bras > 1){
matthieuvignon 2:9d280856a536 212 Initialisation_position(choix_bras);
matthieuvignon 2:9d280856a536 213 }
matthieuvignon 2:9d280856a536 214 break;
matthieuvignon 2:9d280856a536 215
R66Y 3:1bb26049bdd1 216 case AX12_PREPARATION_PRISE : // 1 (10)
matthieuvignon 2:9d280856a536 217 Preparation_prise(choix_bras);
matthieuvignon 2:9d280856a536 218 if (action == 0){
matthieuvignon 2:9d280856a536 219 Fin_action();
matthieuvignon 2:9d280856a536 220 action ++;
matthieuvignon 2:9d280856a536 221 }
matthieuvignon 2:9d280856a536 222 break;
matthieuvignon 2:9d280856a536 223
R66Y 3:1bb26049bdd1 224 case AX12_STOCKAGE_HAUT : // 2 (10)
matthieuvignon 2:9d280856a536 225 Stockage_haut(choix_bras);
matthieuvignon 2:9d280856a536 226 etat_ax12 = AX12_DEFAUT;
matthieuvignon 2:9d280856a536 227 Fin_action();
matthieuvignon 2:9d280856a536 228 break;
matthieuvignon 2:9d280856a536 229
R66Y 3:1bb26049bdd1 230 case AX12_STOCKAGE_BAS : // 3 (10)
matthieuvignon 2:9d280856a536 231 Stockage_bas(choix_bras);
matthieuvignon 2:9d280856a536 232 etat_ax12 = AX12_DEFAUT;
matthieuvignon 2:9d280856a536 233 Fin_action();
matthieuvignon 2:9d280856a536 234 break;
matthieuvignon 2:9d280856a536 235
R66Y 3:1bb26049bdd1 236 case AX12_DEPOSER : // 4 (10)
matthieuvignon 2:9d280856a536 237 Deposer(choix_bras);
matthieuvignon 2:9d280856a536 238 etat_ax12 = AX12_DEFAUT;
matthieuvignon 2:9d280856a536 239 Fin_action();
matthieuvignon 2:9d280856a536 240 break;
matthieuvignon 2:9d280856a536 241
R66Y 3:1bb26049bdd1 242 case AX12_PREPARATION_DEPOT_BAS : // 5 (10)
matthieuvignon 2:9d280856a536 243 Preparation_depot_bas(choix_bras);
matthieuvignon 2:9d280856a536 244 etat_ax12 = AX12_DEFAUT;
matthieuvignon 2:9d280856a536 245 Fin_action();
matthieuvignon 2:9d280856a536 246 break;
matthieuvignon 2:9d280856a536 247
R66Y 3:1bb26049bdd1 248 case AX12_PREPARATION_DEPOT_HAUT : // 6 (10)
matthieuvignon 2:9d280856a536 249 Preparation_depot_haut(choix_bras);
matthieuvignon 2:9d280856a536 250 etat_ax12 = AX12_DEFAUT;
matthieuvignon 2:9d280856a536 251 Fin_action();
matthieuvignon 2:9d280856a536 252 break;
matthieuvignon 2:9d280856a536 253
R66Y 3:1bb26049bdd1 254 case AX12_POUSSER_MODULE : // 7 (10)
matthieuvignon 2:9d280856a536 255 Pousser_module(choix_bras);
matthieuvignon 2:9d280856a536 256 etat_ax12 = AX12_DEFAUT;
matthieuvignon 2:9d280856a536 257 Fin_action();
matthieuvignon 2:9d280856a536 258 break;
matthieuvignon 2:9d280856a536 259
R66Y 3:1bb26049bdd1 260 case BOUGER_MX12 : // 12 (10)
R66Y 3:1bb26049bdd1 261 bouger_MX12(choix_bras); // Le choix du bras détermine le sens de rotation de l'MX12 : 0 position initiale | 1 tourne à droite | 2 tourne à gauche.
R66Y 3:1bb26049bdd1 262 etat_ax12 = AX12_DEFAUT;
R66Y 3:1bb26049bdd1 263 Fin_action();
R66Y 3:1bb26049bdd1 264 break;
R66Y 3:1bb26049bdd1 265
R66Y 3:1bb26049bdd1 266 case AX12_DEFAUT : // 20 (10)
matthieuvignon 2:9d280856a536 267 action = 0;
matthieuvignon 2:9d280856a536 268 break;
matthieuvignon 2:9d280856a536 269 }
matthieuvignon 2:9d280856a536 270 }
matthieuvignon 2:9d280856a536 271
matthieuvignon 2:9d280856a536 272
matthieuvignon 2:9d280856a536 273 unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
matthieuvignon 2:9d280856a536 274 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
matthieuvignon 2:9d280856a536 275 unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3)
matthieuvignon 2:9d280856a536 276 {
matthieuvignon 2:9d280856a536 277 char TabPompeBras[15];
matthieuvignon 2:9d280856a536 278 unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
matthieuvignon 2:9d280856a536 279
matthieuvignon 2:9d280856a536 280 GPosition1_1=((unsigned long)GPosition1*341/1000);
matthieuvignon 2:9d280856a536 281 GPosition2_1=((unsigned long)GPosition2*341/1000);
matthieuvignon 2:9d280856a536 282 GPosition3_1=((unsigned long)GPosition3*341/1000);
matthieuvignon 2:9d280856a536 283
matthieuvignon 2:9d280856a536 284 TabPompeBras[0] = ID1;
matthieuvignon 2:9d280856a536 285 TabPompeBras[1] = GPosition1_1;
matthieuvignon 2:9d280856a536 286 TabPompeBras[2] = GPosition1_1>>8;
matthieuvignon 2:9d280856a536 287 TabPompeBras[3] = GSpeed1;
matthieuvignon 2:9d280856a536 288 TabPompeBras[4] = GSpeed1>>8; /// (haut, milieu, bas)
matthieuvignon 2:9d280856a536 289
matthieuvignon 2:9d280856a536 290 TabPompeBras[5] = ID2;
matthieuvignon 2:9d280856a536 291 TabPompeBras[6] = GPosition2_1;
matthieuvignon 2:9d280856a536 292 TabPompeBras[7] = GPosition2_1>>8;
matthieuvignon 2:9d280856a536 293 TabPompeBras[8] = GSpeed2;
matthieuvignon 2:9d280856a536 294 TabPompeBras[9] = GSpeed2>>8;
matthieuvignon 2:9d280856a536 295
matthieuvignon 2:9d280856a536 296 TabPompeBras[10] = ID3;
matthieuvignon 2:9d280856a536 297 TabPompeBras[11] = GPosition3_1;
matthieuvignon 2:9d280856a536 298 TabPompeBras[12] = GPosition3_1>>8;
matthieuvignon 2:9d280856a536 299 TabPompeBras[13] = GSpeed3;
matthieuvignon 2:9d280856a536 300 TabPompeBras[14] = GSpeed3>>8 ;
matthieuvignon 2:9d280856a536 301
matthieuvignon 2:9d280856a536 302 BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
matthieuvignon 2:9d280856a536 303 wait(TIME);
matthieuvignon 2:9d280856a536 304
matthieuvignon 2:9d280856a536 305
matthieuvignon 2:9d280856a536 306 while (((unsigned short)(HautBrasPompe->Get_Position()*10)>GPosition3*105/100) || ((unsigned short)(HautBrasPompe->Get_Position()*10)<GPosition3*95/100)) {
matthieuvignon 2:9d280856a536 307 BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
matthieuvignon 2:9d280856a536 308 wait(TIME*5);
matthieuvignon 2:9d280856a536 309 }
matthieuvignon 2:9d280856a536 310
matthieuvignon 2:9d280856a536 311 while (((unsigned short)(MilieuBrasPompe->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(MilieuBrasPompe->Get_Position()*10)<GPosition2*95/100)) {
matthieuvignon 2:9d280856a536 312 BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
matthieuvignon 2:9d280856a536 313 wait(TIME*5);
matthieuvignon 2:9d280856a536 314 }
matthieuvignon 2:9d280856a536 315
matthieuvignon 2:9d280856a536 316
matthieuvignon 2:9d280856a536 317 while (((unsigned short)(BasBrasPompe->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(BasBrasPompe->Get_Position()*10)<GPosition1*95/100)) {
matthieuvignon 2:9d280856a536 318 BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
matthieuvignon 2:9d280856a536 319 wait(TIME*5);
matthieuvignon 2:9d280856a536 320 }
matthieuvignon 2:9d280856a536 321
matthieuvignon 2:9d280856a536 322
matthieuvignon 2:9d280856a536 323
matthieuvignon 2:9d280856a536 324 }
matthieuvignon 2:9d280856a536 325
matthieuvignon 2:9d280856a536 326
matthieuvignon 2:9d280856a536 327
matthieuvignon 2:9d280856a536 328 /****************************************************************************************/
matthieuvignon 2:9d280856a536 329 /* FUNCTION NAME: Initialisation_position */
matthieuvignon 2:9d280856a536 330 /* DESCRIPTION : Fonction qui place les bras en position verticale */
matthieuvignon 2:9d280856a536 331 /****************************************************************************************/
matthieuvignon 2:9d280856a536 332 void Initialisation_position(unsigned char choix){
R66Y 4:0972025782f3 333 TrappeLanceur->Set_Secure_Goal(600); // Initialise la position de l'MX12 à 180°
matthieuvignon 2:9d280856a536 334 if (choix == 1){
matthieuvignon 2:9d280856a536 335 multiple_myAX12->multiple_goal_and_speed(4,TAB1);
matthieuvignon 2:9d280856a536 336 wait(TIME);
matthieuvignon 2:9d280856a536 337 }
matthieuvignon 2:9d280856a536 338
matthieuvignon 2:9d280856a536 339 else if (choix == 2){
matthieuvignon 2:9d280856a536 340 multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
matthieuvignon 2:9d280856a536 341 wait(TIME);
matthieuvignon 2:9d280856a536 342 }
matthieuvignon 2:9d280856a536 343 else if (choix == 3){
matthieuvignon 2:9d280856a536 344 BrasPompeAx12->multiple_goal_and_speed(3,TabBrasPompePosInit);
matthieuvignon 2:9d280856a536 345 wait(TIME);
R66Y 3:1bb26049bdd1 346 }
matthieuvignon 2:9d280856a536 347 }
matthieuvignon 2:9d280856a536 348
matthieuvignon 2:9d280856a536 349 /****************************************************************************************/
matthieuvignon 2:9d280856a536 350 /* FUNCTION NAME: Preparation_prise */
matthieuvignon 2:9d280856a536 351 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
matthieuvignon 2:9d280856a536 352 /****************************************************************************************/
matthieuvignon 2:9d280856a536 353 void Preparation_prise(unsigned char choix){
matthieuvignon 2:9d280856a536 354 if (choix == 1){
matthieuvignon 2:9d280856a536 355 multiple_myAX12->multiple_goal_and_speed(4,TAB2);
matthieuvignon 2:9d280856a536 356 wait(TIME);
matthieuvignon 2:9d280856a536 357 }
matthieuvignon 2:9d280856a536 358
matthieuvignon 2:9d280856a536 359 else if (choix == 2){
matthieuvignon 2:9d280856a536 360 multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
matthieuvignon 2:9d280856a536 361 wait(TIME);
matthieuvignon 2:9d280856a536 362 }
matthieuvignon 2:9d280856a536 363 }
matthieuvignon 2:9d280856a536 364
matthieuvignon 2:9d280856a536 365 /****************************************************************************************/
matthieuvignon 2:9d280856a536 366 /* FUNCTION NAME: Stockage_haut */
matthieuvignon 2:9d280856a536 367 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
matthieuvignon 2:9d280856a536 368 /****************************************************************************************/
matthieuvignon 2:9d280856a536 369 void Stockage_haut(unsigned char choix){
matthieuvignon 2:9d280856a536 370 if (choix == 1){
matthieuvignon 2:9d280856a536 371 multiple_myAX12->multiple_goal_and_speed(4,TAB3);
matthieuvignon 2:9d280856a536 372 wait(TIME);
matthieuvignon 2:9d280856a536 373 multiple_myAX12->multiple_goal_and_speed(4,TAB4);
matthieuvignon 2:9d280856a536 374 wait(TIME);
matthieuvignon 2:9d280856a536 375 multiple_myAX12->multiple_goal_and_speed(4,TAB5);
matthieuvignon 2:9d280856a536 376 wait(TIME);
matthieuvignon 2:9d280856a536 377 multiple_myAX12->multiple_goal_and_speed(4,TAB6);
matthieuvignon 2:9d280856a536 378 wait(TIME);
matthieuvignon 2:9d280856a536 379 }
matthieuvignon 2:9d280856a536 380
matthieuvignon 2:9d280856a536 381 else if (choix == 2){
matthieuvignon 2:9d280856a536 382 multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
matthieuvignon 2:9d280856a536 383 wait(TIME);
matthieuvignon 2:9d280856a536 384 multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
matthieuvignon 2:9d280856a536 385 wait(TIME);
matthieuvignon 2:9d280856a536 386 multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
matthieuvignon 2:9d280856a536 387 wait(TIME);
matthieuvignon 2:9d280856a536 388 multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
matthieuvignon 2:9d280856a536 389 wait(TIME);
matthieuvignon 2:9d280856a536 390 }
matthieuvignon 2:9d280856a536 391 }
matthieuvignon 2:9d280856a536 392
matthieuvignon 2:9d280856a536 393 /****************************************************************************************/
matthieuvignon 2:9d280856a536 394 /* FUNCTION NAME: Stockage_bas */
matthieuvignon 2:9d280856a536 395 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
matthieuvignon 2:9d280856a536 396 /****************************************************************************************/
matthieuvignon 2:9d280856a536 397 void Stockage_bas(unsigned char choix){
matthieuvignon 2:9d280856a536 398 if (choix == 1){
matthieuvignon 2:9d280856a536 399 multiple_myAX12->multiple_goal_and_speed(4,TAB3);
matthieuvignon 2:9d280856a536 400 wait(TIME);
matthieuvignon 2:9d280856a536 401 multiple_myAX12->multiple_goal_and_speed(4,TAB7);
matthieuvignon 2:9d280856a536 402 wait(TIME);
matthieuvignon 2:9d280856a536 403 }
matthieuvignon 2:9d280856a536 404
matthieuvignon 2:9d280856a536 405 else if (choix == 2){
matthieuvignon 2:9d280856a536 406 multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
matthieuvignon 2:9d280856a536 407 wait(TIME);
matthieuvignon 2:9d280856a536 408 multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
matthieuvignon 2:9d280856a536 409 wait(TIME);
matthieuvignon 2:9d280856a536 410 }
matthieuvignon 2:9d280856a536 411 }
matthieuvignon 2:9d280856a536 412
matthieuvignon 2:9d280856a536 413 /****************************************************************************************/
matthieuvignon 2:9d280856a536 414 /* FUNCTION NAME: Deposer */
matthieuvignon 2:9d280856a536 415 /* DESCRIPTION : Fonction qui permet de déposer le module */
matthieuvignon 2:9d280856a536 416 /****************************************************************************************/
matthieuvignon 2:9d280856a536 417 void Deposer(unsigned char choix){
matthieuvignon 2:9d280856a536 418 if (choix == 1){
matthieuvignon 2:9d280856a536 419 multiple_myAX12->multiple_goal_and_speed(4,TAB9);
matthieuvignon 2:9d280856a536 420 wait(TIME);
matthieuvignon 2:9d280856a536 421 }
matthieuvignon 2:9d280856a536 422
matthieuvignon 2:9d280856a536 423 else if (choix == 2){
matthieuvignon 2:9d280856a536 424 multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
matthieuvignon 2:9d280856a536 425 wait(TIME);
matthieuvignon 2:9d280856a536 426 }
matthieuvignon 2:9d280856a536 427 }
matthieuvignon 2:9d280856a536 428
matthieuvignon 2:9d280856a536 429 /****************************************************************************************/
matthieuvignon 2:9d280856a536 430 /* FUNCTION NAME: Preparation_depot_bas */
matthieuvignon 2:9d280856a536 431 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
matthieuvignon 2:9d280856a536 432 /****************************************************************************************/
matthieuvignon 2:9d280856a536 433 void Preparation_depot_bas(unsigned char choix){
matthieuvignon 2:9d280856a536 434 if (choix == 1){
matthieuvignon 2:9d280856a536 435 multiple_myAX12->multiple_goal_and_speed(4,TAB8);
matthieuvignon 2:9d280856a536 436 wait(TIME);
matthieuvignon 2:9d280856a536 437 }
matthieuvignon 2:9d280856a536 438
matthieuvignon 2:9d280856a536 439 else if (choix == 2){
matthieuvignon 2:9d280856a536 440 multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
matthieuvignon 2:9d280856a536 441 wait(TIME);
matthieuvignon 2:9d280856a536 442 }
matthieuvignon 2:9d280856a536 443 }
matthieuvignon 2:9d280856a536 444
matthieuvignon 2:9d280856a536 445 /****************************************************************************************/
matthieuvignon 2:9d280856a536 446 /* FUNCTION NAME: Preparation_depot_haut */
matthieuvignon 2:9d280856a536 447 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
matthieuvignon 2:9d280856a536 448 /****************************************************************************************/
matthieuvignon 2:9d280856a536 449 void Preparation_depot_haut(unsigned char choix){
matthieuvignon 2:9d280856a536 450 if (choix == 1){
matthieuvignon 2:9d280856a536 451 multiple_myAX12->multiple_goal_and_speed(4,TAB6);
matthieuvignon 2:9d280856a536 452 wait(TIME);
matthieuvignon 2:9d280856a536 453 multiple_myAX12->multiple_goal_and_speed(4,TAB5);
matthieuvignon 2:9d280856a536 454 wait(TIME);
matthieuvignon 2:9d280856a536 455 multiple_myAX12->multiple_goal_and_speed(4,TAB10);
matthieuvignon 2:9d280856a536 456 wait(TIME);
matthieuvignon 2:9d280856a536 457 multiple_myAX12->multiple_goal_and_speed(4,TAB8);
matthieuvignon 2:9d280856a536 458 wait(TIME);
matthieuvignon 2:9d280856a536 459 }
matthieuvignon 2:9d280856a536 460
matthieuvignon 2:9d280856a536 461 else if (choix == 2){
matthieuvignon 2:9d280856a536 462 multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
matthieuvignon 2:9d280856a536 463 wait(TIME);
matthieuvignon 2:9d280856a536 464 multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
matthieuvignon 2:9d280856a536 465 wait(TIME);
matthieuvignon 2:9d280856a536 466 multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
matthieuvignon 2:9d280856a536 467 wait(TIME);
matthieuvignon 2:9d280856a536 468 multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
matthieuvignon 2:9d280856a536 469 wait(TIME);
matthieuvignon 2:9d280856a536 470 }
matthieuvignon 2:9d280856a536 471 }
matthieuvignon 2:9d280856a536 472
matthieuvignon 2:9d280856a536 473 /****************************************************************************************/
R66Y 3:1bb26049bdd1 474 /* FUNCTION NAME: bouger_MX12 */
R66Y 3:1bb26049bdd1 475 /* DESCRIPTION : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge */
R66Y 3:1bb26049bdd1 476 /****************************************************************************************/
R66Y 3:1bb26049bdd1 477 void bouger_MX12(unsigned char choix){
R66Y 3:1bb26049bdd1 478 if( choix == 1) // Tourne à droite
R66Y 3:1bb26049bdd1 479 {
R66Y 4:0972025782f3 480 TrappeLanceur->Set_Secure_Goal(0); // tourner droite
R66Y 4:0972025782f3 481 wait(TIME);
R66Y 3:1bb26049bdd1 482 }
R66Y 3:1bb26049bdd1 483 else if(choix == 2) // Tourne à gauche
R66Y 3:1bb26049bdd1 484 {
R66Y 4:0972025782f3 485 TrappeLanceur->Set_Secure_Goal(1200); // tourner gauche
R66Y 4:0972025782f3 486 wait(TIME);
R66Y 3:1bb26049bdd1 487 }
R66Y 3:1bb26049bdd1 488 else if (choix == 0)
R66Y 3:1bb26049bdd1 489 {
R66Y 4:0972025782f3 490 TrappeLanceur->Set_Secure_Goal(600); // position initiale
R66Y 4:0972025782f3 491 wait(TIME);
R66Y 3:1bb26049bdd1 492 }
R66Y 3:1bb26049bdd1 493 }
R66Y 3:1bb26049bdd1 494
R66Y 3:1bb26049bdd1 495
R66Y 3:1bb26049bdd1 496 /****************************************************************************************/
matthieuvignon 2:9d280856a536 497 /* FUNCTION NAME: Pousser_module */
matthieuvignon 2:9d280856a536 498 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
matthieuvignon 2:9d280856a536 499 /****************************************************************************************/
matthieuvignon 2:9d280856a536 500 void Pousser_module(unsigned char choix){
matthieuvignon 2:9d280856a536 501 if (choix == 1){
matthieuvignon 2:9d280856a536 502 multiple_myAX12->multiple_goal_and_speed(4,TAB11);
matthieuvignon 2:9d280856a536 503 wait(TIME);
matthieuvignon 2:9d280856a536 504 }
matthieuvignon 2:9d280856a536 505
matthieuvignon 2:9d280856a536 506 else if (choix == 2){
matthieuvignon 2:9d280856a536 507 multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
matthieuvignon 2:9d280856a536 508 wait(TIME);
matthieuvignon 2:9d280856a536 509 }
matthieuvignon 2:9d280856a536 510 }
matthieuvignon 2:9d280856a536 511
matthieuvignon 2:9d280856a536 512 /****************************************************************************************/
matthieuvignon 2:9d280856a536 513 /* FUNCTION NAME: Fin_action */
matthieuvignon 2:9d280856a536 514 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
matthieuvignon 2:9d280856a536 515 /****************************************************************************************/
matthieuvignon 2:9d280856a536 516 void Fin_action(void){
matthieuvignon 2:9d280856a536 517 CANMessage msgTx=CANMessage();
matthieuvignon 2:9d280856a536 518 msgTx.format=CANStandard;
matthieuvignon 2:9d280856a536 519 msgTx.type=CANData;
matthieuvignon 2:9d280856a536 520
matthieuvignon 2:9d280856a536 521 msgTx.id = SERVO_AX12_END;
matthieuvignon 2:9d280856a536 522 msgTx.len = 1;
matthieuvignon 2:9d280856a536 523 msgTx.data[0] = AX12_PREPARATION_PRISE;
matthieuvignon 2:9d280856a536 524 can.write(msgTx);
matthieuvignon 2:9d280856a536 525 }
matthieuvignon 2:9d280856a536 526
matthieuvignon 2:9d280856a536 527
matthieuvignon 2:9d280856a536 528
matthieuvignon 2:9d280856a536 529
matthieuvignon 2:9d280856a536 530 /****************************************************************************************/
matthieuvignon 2:9d280856a536 531 /* FUNCTION NAME: Check_positionAX12 */
matthieuvignon 2:9d280856a536 532 /* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */
matthieuvignon 2:9d280856a536 533 /****************************************************************************************/
matthieuvignon 2:9d280856a536 534 void Check_positionAX12(char* TAB, unsigned char choix){
matthieuvignon 2:9d280856a536 535 int k=1, i=0;
matthieuvignon 2:9d280856a536 536 static float TAB_POS_TH[4];
matthieuvignon 2:9d280856a536 537
matthieuvignon 2:9d280856a536 538 CANMessage msgTx=CANMessage();
matthieuvignon 2:9d280856a536 539 msgTx.id=SERVO_AX12_POSITION;
matthieuvignon 2:9d280856a536 540 msgTx.len=5;
matthieuvignon 2:9d280856a536 541
matthieuvignon 2:9d280856a536 542 //PERMET DE VERIFIER LA POSITION D'UN AX12
matthieuvignon 2:9d280856a536 543 TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 544 TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 545 TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 546 TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 547 TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 548 TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 549 TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 550 TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3);
matthieuvignon 2:9d280856a536 551
matthieuvignon 2:9d280856a536 552 for(i=0; i<4; i++){
matthieuvignon 2:9d280856a536 553 TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8);
matthieuvignon 2:9d280856a536 554 k=k+5;
matthieuvignon 2:9d280856a536 555 }
matthieuvignon 2:9d280856a536 556
matthieuvignon 2:9d280856a536 557 for(i=0; i<4; i++){
matthieuvignon 2:9d280856a536 558 if (choix == 1){
matthieuvignon 2:9d280856a536 559 if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
matthieuvignon 2:9d280856a536 560 TAB_POSITION[i] = 1;
matthieuvignon 2:9d280856a536 561 }
matthieuvignon 2:9d280856a536 562 else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
matthieuvignon 2:9d280856a536 563 TAB_POSITION[i] = 0;
matthieuvignon 2:9d280856a536 564 }
matthieuvignon 2:9d280856a536 565 }
matthieuvignon 2:9d280856a536 566 else if (choix == 2){
matthieuvignon 2:9d280856a536 567 if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
matthieuvignon 2:9d280856a536 568 TAB_POSITION[i] = 1;
matthieuvignon 2:9d280856a536 569 }
matthieuvignon 2:9d280856a536 570 else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
matthieuvignon 2:9d280856a536 571 TAB_POSITION[i] = 0;
matthieuvignon 2:9d280856a536 572 }
matthieuvignon 2:9d280856a536 573 }
matthieuvignon 2:9d280856a536 574 }
matthieuvignon 2:9d280856a536 575
matthieuvignon 2:9d280856a536 576 msgTx.data[0] = choix;
matthieuvignon 2:9d280856a536 577 for(i=1; i<5; i++){
matthieuvignon 2:9d280856a536 578 msgTx.data[i] = TAB_POSITION[i];
matthieuvignon 2:9d280856a536 579 }
matthieuvignon 2:9d280856a536 580 can.write(msgTx);
matthieuvignon 2:9d280856a536 581 }
matthieuvignon 2:9d280856a536 582