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@2:9d280856a536, 2017-05-19 (annotated)

Committer:
matthieuvignon
Date:
Fri May 19 08:06:49 2017 +0000
Revision:
2:9d280856a536
Child:
3:1bb26049bdd1
Programme AX12 + verification position

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