Programme d'utilisation des AX12 avec rajout de l'MX12
Fork of test_carteAToutFaire_PR by
Actionneur.cpp@3:1bb26049bdd1, 2017-05-20 (annotated)
- Committer:
- R66Y
- Date:
- Sat May 20 15:23:58 2017 +0000
- Revision:
- 3:1bb26049bdd1
- Parent:
- 2:9d280856a536
Programme de contr?le des AX12 avec rajout de l'MX12;
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:1bb26049bdd1 | 333 | un_myMX12->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 | 3:1bb26049bdd1 | 480 | un_myMX12->Set_Secure_Goal(0); // tourner droite |
R66Y | 3:1bb26049bdd1 | 481 | } |
R66Y | 3:1bb26049bdd1 | 482 | else if(choix == 2) // Tourne à gauche |
R66Y | 3:1bb26049bdd1 | 483 | { |
R66Y | 3:1bb26049bdd1 | 484 | un_myMX12->Set_Secure_Goal(1200); // tourner gauche |
R66Y | 3:1bb26049bdd1 | 485 | } |
R66Y | 3:1bb26049bdd1 | 486 | else if (choix == 0) |
R66Y | 3:1bb26049bdd1 | 487 | { |
R66Y | 3:1bb26049bdd1 | 488 | un_myMX12->Set_Secure_Goal(600); // position initiale |
R66Y | 3:1bb26049bdd1 | 489 | } |
R66Y | 3:1bb26049bdd1 | 490 | } |
R66Y | 3:1bb26049bdd1 | 491 | |
R66Y | 3:1bb26049bdd1 | 492 | |
R66Y | 3:1bb26049bdd1 | 493 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 494 | /* FUNCTION NAME: Pousser_module */ |
matthieuvignon | 2:9d280856a536 | 495 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
matthieuvignon | 2:9d280856a536 | 496 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 497 | void Pousser_module(unsigned char choix){ |
matthieuvignon | 2:9d280856a536 | 498 | if (choix == 1){ |
matthieuvignon | 2:9d280856a536 | 499 | multiple_myAX12->multiple_goal_and_speed(4,TAB11); |
matthieuvignon | 2:9d280856a536 | 500 | wait(TIME); |
matthieuvignon | 2:9d280856a536 | 501 | } |
matthieuvignon | 2:9d280856a536 | 502 | |
matthieuvignon | 2:9d280856a536 | 503 | else if (choix == 2){ |
matthieuvignon | 2:9d280856a536 | 504 | multiple2_myAX12->multiple_goal_and_speed(4,TAB31); |
matthieuvignon | 2:9d280856a536 | 505 | wait(TIME); |
matthieuvignon | 2:9d280856a536 | 506 | } |
matthieuvignon | 2:9d280856a536 | 507 | } |
matthieuvignon | 2:9d280856a536 | 508 | |
matthieuvignon | 2:9d280856a536 | 509 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 510 | /* FUNCTION NAME: Fin_action */ |
matthieuvignon | 2:9d280856a536 | 511 | /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ |
matthieuvignon | 2:9d280856a536 | 512 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 513 | void Fin_action(void){ |
matthieuvignon | 2:9d280856a536 | 514 | CANMessage msgTx=CANMessage(); |
matthieuvignon | 2:9d280856a536 | 515 | msgTx.format=CANStandard; |
matthieuvignon | 2:9d280856a536 | 516 | msgTx.type=CANData; |
matthieuvignon | 2:9d280856a536 | 517 | |
matthieuvignon | 2:9d280856a536 | 518 | msgTx.id = SERVO_AX12_END; |
matthieuvignon | 2:9d280856a536 | 519 | msgTx.len = 1; |
matthieuvignon | 2:9d280856a536 | 520 | msgTx.data[0] = AX12_PREPARATION_PRISE; |
matthieuvignon | 2:9d280856a536 | 521 | can.write(msgTx); |
matthieuvignon | 2:9d280856a536 | 522 | } |
matthieuvignon | 2:9d280856a536 | 523 | |
matthieuvignon | 2:9d280856a536 | 524 | |
matthieuvignon | 2:9d280856a536 | 525 | |
matthieuvignon | 2:9d280856a536 | 526 | |
matthieuvignon | 2:9d280856a536 | 527 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 528 | /* FUNCTION NAME: Check_positionAX12 */ |
matthieuvignon | 2:9d280856a536 | 529 | /* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */ |
matthieuvignon | 2:9d280856a536 | 530 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 531 | void Check_positionAX12(char* TAB, unsigned char choix){ |
matthieuvignon | 2:9d280856a536 | 532 | int k=1, i=0; |
matthieuvignon | 2:9d280856a536 | 533 | static float TAB_POS_TH[4]; |
matthieuvignon | 2:9d280856a536 | 534 | |
matthieuvignon | 2:9d280856a536 | 535 | CANMessage msgTx=CANMessage(); |
matthieuvignon | 2:9d280856a536 | 536 | msgTx.id=SERVO_AX12_POSITION; |
matthieuvignon | 2:9d280856a536 | 537 | msgTx.len=5; |
matthieuvignon | 2:9d280856a536 | 538 | |
matthieuvignon | 2:9d280856a536 | 539 | //PERMET DE VERIFIER LA POSITION D'UN AX12 |
matthieuvignon | 2:9d280856a536 | 540 | TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 541 | TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 542 | TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 543 | TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 544 | TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 545 | TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 546 | TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 547 | TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3); |
matthieuvignon | 2:9d280856a536 | 548 | |
matthieuvignon | 2:9d280856a536 | 549 | for(i=0; i<4; i++){ |
matthieuvignon | 2:9d280856a536 | 550 | TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8); |
matthieuvignon | 2:9d280856a536 | 551 | k=k+5; |
matthieuvignon | 2:9d280856a536 | 552 | } |
matthieuvignon | 2:9d280856a536 | 553 | |
matthieuvignon | 2:9d280856a536 | 554 | for(i=0; i<4; i++){ |
matthieuvignon | 2:9d280856a536 | 555 | if (choix == 1){ |
matthieuvignon | 2:9d280856a536 | 556 | if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ |
matthieuvignon | 2:9d280856a536 | 557 | TAB_POSITION[i] = 1; |
matthieuvignon | 2:9d280856a536 | 558 | } |
matthieuvignon | 2:9d280856a536 | 559 | else 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] = 0; |
matthieuvignon | 2:9d280856a536 | 561 | } |
matthieuvignon | 2:9d280856a536 | 562 | } |
matthieuvignon | 2:9d280856a536 | 563 | else if (choix == 2){ |
matthieuvignon | 2:9d280856a536 | 564 | if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ |
matthieuvignon | 2:9d280856a536 | 565 | TAB_POSITION[i] = 1; |
matthieuvignon | 2:9d280856a536 | 566 | } |
matthieuvignon | 2:9d280856a536 | 567 | else 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] = 0; |
matthieuvignon | 2:9d280856a536 | 569 | } |
matthieuvignon | 2:9d280856a536 | 570 | } |
matthieuvignon | 2:9d280856a536 | 571 | } |
matthieuvignon | 2:9d280856a536 | 572 | |
matthieuvignon | 2:9d280856a536 | 573 | msgTx.data[0] = choix; |
matthieuvignon | 2:9d280856a536 | 574 | for(i=1; i<5; i++){ |
matthieuvignon | 2:9d280856a536 | 575 | msgTx.data[i] = TAB_POSITION[i]; |
matthieuvignon | 2:9d280856a536 | 576 | } |
matthieuvignon | 2:9d280856a536 | 577 | can.write(msgTx); |
matthieuvignon | 2:9d280856a536 | 578 | } |
matthieuvignon | 2:9d280856a536 | 579 |