Programme d'utilisation des AX12 et de l'MX12 V3
Fork of Utilisatio_MX12_V2 by
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?
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]; |
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 |