CRAC Team / Utilisatio_MX12_V4

Programme d'utilisation des AX12 et de l'MX12 V3. 0C = action de l'MX12. (data0) 0 | 1 | 2 = position & sens de rotation

Dependencies:   MX12

Fork of Utilisatio_MX12_V3 by CRAC Team

Actionneur.cpp@5:bb953eda06e7, 2017-05-20 (annotated)

Committer:
R66Y
Date:
Sat May 20 18:12:28 2017 +0000
Revision:
5:bb953eda06e7
Parent:
4:0972025782f3 
Derni?re version de l'MX12.;

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