mouvement AX12 petit robot version 2
Fork of command_AX12_petit_robot_v2 by
actionneurs/actionneurs.cpp@7:ad4a19e26b84, 2017-05-12 (annotated)
- Committer:
- ClementBreteau
- Date:
- Fri May 12 14:35:09 2017 +0000
- Revision:
- 7:ad4a19e26b84
position ax12
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 7:ad4a19e26b84 | 1 | #include"AX12.h" |
ClementBreteau | 7:ad4a19e26b84 | 2 | #include"mbed.h" |
ClementBreteau | 7:ad4a19e26b84 | 3 | #include "global.h" |
ClementBreteau | 7:ad4a19e26b84 | 4 | |
ClementBreteau | 7:ad4a19e26b84 | 5 | #define TIME 0.8 |
ClementBreteau | 7:ad4a19e26b84 | 6 | #define VITESSE 700 |
ClementBreteau | 7:ad4a19e26b84 | 7 | DigitalOut led(LED1); |
ClementBreteau | 7:ad4a19e26b84 | 8 | |
ClementBreteau | 7:ad4a19e26b84 | 9 | // AX12 partie centrale du petit robot |
ClementBreteau | 7:ad4a19e26b84 | 10 | AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, *multipleBrasCentral; |
ClementBreteau | 7:ad4a19e26b84 | 11 | AX12 *partieDoigtCentre, *multipleDoigtCentre; |
ClementBreteau | 7:ad4a19e26b84 | 12 | // AX12 partie gauche du robot |
ClementBreteau | 7:ad4a19e26b84 | 13 | AX12 *partieBasseGauche, *partieMainGauche, *multipleBrasGauche; |
ClementBreteau | 7:ad4a19e26b84 | 14 | AX12 *partiePorteGauche, *multiplePorteGauche; |
ClementBreteau | 7:ad4a19e26b84 | 15 | // AX12 partie droite du robot |
ClementBreteau | 7:ad4a19e26b84 | 16 | AX12 *partieBasseDroite, *partieMainDroite, *multipleBrasDroite; |
ClementBreteau | 7:ad4a19e26b84 | 17 | AX12 *partiePorteDroite, *multiplePorteDroite; |
ClementBreteau | 7:ad4a19e26b84 | 18 | |
ClementBreteau | 7:ad4a19e26b84 | 19 | Serial pc(USBTX, USBRX); |
ClementBreteau | 7:ad4a19e26b84 | 20 | |
ClementBreteau | 7:ad4a19e26b84 | 21 | Timer t; |
ClementBreteau | 7:ad4a19e26b84 | 22 | Ticker flipper; |
ClementBreteau | 7:ad4a19e26b84 | 23 | unsigned char action_precedente = 0; |
ClementBreteau | 7:ad4a19e26b84 | 24 | ////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// |
ClementBreteau | 7:ad4a19e26b84 | 25 | static char TAB1[25]= {0x01,0x15, 0x02, 0xFF, 0x00, ///Position initiale |
ClementBreteau | 7:ad4a19e26b84 | 26 | 0x10,0x00, 0x02, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 27 | 0x07,0x90, 0x01, 0xB1, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 28 | 0x08,0x58, 0x02, 0x79, 0x00}; |
ClementBreteau | 7:ad4a19e26b84 | 29 | |
ClementBreteau | 7:ad4a19e26b84 | 30 | static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x02, ///Preparation prise |
ClementBreteau | 7:ad4a19e26b84 | 31 | 0x10,0x50, 0x01, 0xFF, 0x02, |
ClementBreteau | 7:ad4a19e26b84 | 32 | 0x07,0xF4, 0x01, 0xFF, 0x02, |
ClementBreteau | 7:ad4a19e26b84 | 33 | 0x08,0xF4, 0x01, 0xFF, 0x02}; |
ClementBreteau | 7:ad4a19e26b84 | 34 | |
ClementBreteau | 7:ad4a19e26b84 | 35 | static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee) |
ClementBreteau | 7:ad4a19e26b84 | 36 | 0x10,0x50, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 37 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 38 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 39 | |
ClementBreteau | 7:ad4a19e26b84 | 40 | static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air) |
ClementBreteau | 7:ad4a19e26b84 | 41 | 0x10,0x50, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 42 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 43 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 44 | |
ClementBreteau | 7:ad4a19e26b84 | 45 | static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) |
ClementBreteau | 7:ad4a19e26b84 | 46 | 0x10,0xF4, 0x01, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 47 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 48 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 49 | |
ClementBreteau | 7:ad4a19e26b84 | 50 | static char TAB6[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
ClementBreteau | 7:ad4a19e26b84 | 51 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 52 | 0x07,0x4d, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 53 | 0x08,0x9a, 0x02, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 54 | |
ClementBreteau | 7:ad4a19e26b84 | 55 | static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air) |
ClementBreteau | 7:ad4a19e26b84 | 56 | 0x10,0xB0, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 57 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 58 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
ClementBreteau | 7:ad4a19e26b84 | 59 | |
ClementBreteau | 7:ad4a19e26b84 | 60 | static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas |
ClementBreteau | 7:ad4a19e26b84 | 61 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 62 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 63 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
ClementBreteau | 7:ad4a19e26b84 | 64 | |
ClementBreteau | 7:ad4a19e26b84 | 65 | static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer |
ClementBreteau | 7:ad4a19e26b84 | 66 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 67 | 0x07,0xD0, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 68 | 0x08,0x35, 0x03, 0xFF, 0x00}; |
ClementBreteau | 7:ad4a19e26b84 | 69 | |
ClementBreteau | 7:ad4a19e26b84 | 70 | static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) |
ClementBreteau | 7:ad4a19e26b84 | 71 | 0x10,0x00, 0x01, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 72 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 73 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 74 | |
ClementBreteau | 7:ad4a19e26b84 | 75 | static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module |
ClementBreteau | 7:ad4a19e26b84 | 76 | 0x10,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 77 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 78 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
ClementBreteau | 7:ad4a19e26b84 | 79 | |
ClementBreteau | 7:ad4a19e26b84 | 80 | static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale |
ClementBreteau | 7:ad4a19e26b84 | 81 | 0x10,0x00, 0x02, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 82 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 83 | 0x08,0xF4, 0x01, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 84 | |
ClementBreteau | 7:ad4a19e26b84 | 85 | static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer |
ClementBreteau | 7:ad4a19e26b84 | 86 | 0x10,0xA0, 0x02, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 87 | 0x07,0xD0, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 88 | 0x08,0x35, 0x03, 0xFF, 0x00}; |
ClementBreteau | 7:ad4a19e26b84 | 89 | |
ClementBreteau | 7:ad4a19e26b84 | 90 | static char TAB14[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
ClementBreteau | 7:ad4a19e26b84 | 91 | 0x10,0x42, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 92 | 0x07,0x4d, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 93 | 0x08,0x9a, 0x02, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 94 | |
ClementBreteau | 7:ad4a19e26b84 | 95 | static char TAB15[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (module sur tige) |
ClementBreteau | 7:ad4a19e26b84 | 96 | 0x10,0x42, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 97 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 98 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 99 | |
ClementBreteau | 7:ad4a19e26b84 | 100 | static char TAB16[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
ClementBreteau | 7:ad4a19e26b84 | 101 | 0x10,0x42, 0x00, 0xFF, 0x00, |
ClementBreteau | 7:ad4a19e26b84 | 102 | 0x07,0x2c, 0x01, 0xFF, 0x03, |
ClementBreteau | 7:ad4a19e26b84 | 103 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 104 | ////////////////////// TABLEAU BRAS GAUCHE /////////////////////////// |
ClementBreteau | 7:ad4a19e26b84 | 105 | static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale |
ClementBreteau | 7:ad4a19e26b84 | 106 | 0x02,0xF4, 0x01, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 107 | |
ClementBreteau | 7:ad4a19e26b84 | 108 | static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner |
ClementBreteau | 7:ad4a19e26b84 | 109 | 0x02,0x40, 0x03, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 110 | |
ClementBreteau | 7:ad4a19e26b84 | 111 | static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module |
ClementBreteau | 7:ad4a19e26b84 | 112 | 0x02,0xE5, 0x02, 0xFF, 0x03}; |
ClementBreteau | 7:ad4a19e26b84 | 113 | |
ClementBreteau | 7:ad4a19e26b84 | 114 | |
ClementBreteau | 7:ad4a19e26b84 | 115 | |
ClementBreteau | 7:ad4a19e26b84 | 116 | |
ClementBreteau | 7:ad4a19e26b84 | 117 | /* ANGLE */ |
ClementBreteau | 7:ad4a19e26b84 | 118 | |
ClementBreteau | 7:ad4a19e26b84 | 119 | /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 |
ClementBreteau | 7:ad4a19e26b84 | 120 | 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 |
ClementBreteau | 7:ad4a19e26b84 | 121 | 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 |
ClementBreteau | 7:ad4a19e26b84 | 122 | 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 |
ClementBreteau | 7:ad4a19e26b84 | 123 | 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 |
ClementBreteau | 7:ad4a19e26b84 | 124 | 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 |
ClementBreteau | 7:ad4a19e26b84 | 125 | 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 |
ClementBreteau | 7:ad4a19e26b84 | 126 | 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 |
ClementBreteau | 7:ad4a19e26b84 | 127 | 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 |
ClementBreteau | 7:ad4a19e26b84 | 128 | 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ |
ClementBreteau | 7:ad4a19e26b84 | 129 | |
ClementBreteau | 7:ad4a19e26b84 | 130 | /* NUMERO AX12 */ |
ClementBreteau | 7:ad4a19e26b84 | 131 | |
ClementBreteau | 7:ad4a19e26b84 | 132 | /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 |
ClementBreteau | 7:ad4a19e26b84 | 133 | 1 = 0x01 | 10 = 0x0A |
ClementBreteau | 7:ad4a19e26b84 | 134 | 2 = 0x02 | 11 = 0x0B |
ClementBreteau | 7:ad4a19e26b84 | 135 | 3 = 0x03 | 12 = 0x0C |
ClementBreteau | 7:ad4a19e26b84 | 136 | 4 = 0x04 | 13 = 0x0D |
ClementBreteau | 7:ad4a19e26b84 | 137 | 5 = 0x05 | 14 = 0x0E |
ClementBreteau | 7:ad4a19e26b84 | 138 | 6 = 0x06 | 15 = 0x0F |
ClementBreteau | 7:ad4a19e26b84 | 139 | 7 = 0x07 | 16 = 0x10 |
ClementBreteau | 7:ad4a19e26b84 | 140 | 8 = 0x08 | 17 = 0x11 */ |
ClementBreteau | 7:ad4a19e26b84 | 141 | |
ClementBreteau | 7:ad4a19e26b84 | 142 | |
ClementBreteau | 7:ad4a19e26b84 | 143 | |
ClementBreteau | 7:ad4a19e26b84 | 144 | |
ClementBreteau | 7:ad4a19e26b84 | 145 | |
ClementBreteau | 7:ad4a19e26b84 | 146 | |
ClementBreteau | 7:ad4a19e26b84 | 147 | void declarationAX12(void){ |
ClementBreteau | 7:ad4a19e26b84 | 148 | |
ClementBreteau | 7:ad4a19e26b84 | 149 | //Pince centrale |
ClementBreteau | 7:ad4a19e26b84 | 150 | //AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, *multipleBrasCentral; |
ClementBreteau | 7:ad4a19e26b84 | 151 | partieBasseCentre = new AX12(p9, p10, 1, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 152 | partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 153 | partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 154 | partiePoignetCentre = new AX12(p9, p10, 16, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 155 | multipleBrasCentral = new AX12(p9,p10,0xFE,1000000); |
ClementBreteau | 7:ad4a19e26b84 | 156 | |
ClementBreteau | 7:ad4a19e26b84 | 157 | //Bras de gauche |
ClementBreteau | 7:ad4a19e26b84 | 158 | partieBasseGauche = new AX12(p13, p14, 1, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 159 | partieMainGauche = new AX12(p13, p14, 2, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 160 | multipleBrasGauche = new AX12(p13,p14,0xFE,1000000); |
ClementBreteau | 7:ad4a19e26b84 | 161 | //Porte gauche |
ClementBreteau | 7:ad4a19e26b84 | 162 | partiePorteGauche = new AX12(p13, p14, 3, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 163 | multiplePorteGauche = new AX12(p13, p14, 0xFE, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 164 | |
ClementBreteau | 7:ad4a19e26b84 | 165 | //Bras de droite |
ClementBreteau | 7:ad4a19e26b84 | 166 | partieBasseDroite = new AX12(p28, p27, 1, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 167 | partieMainDroite = new AX12(p28, p27, 2, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 168 | multipleBrasDroite = new AX12(p28,p27,0xFE,1000000); |
ClementBreteau | 7:ad4a19e26b84 | 169 | //Porte droite |
ClementBreteau | 7:ad4a19e26b84 | 170 | partiePorteDroite = new AX12(p28, p27, 3, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 171 | multiplePorteDroite = new AX12(p28, p27, 0xFE, 1000000); |
ClementBreteau | 7:ad4a19e26b84 | 172 | } |
ClementBreteau | 7:ad4a19e26b84 | 173 | |
ClementBreteau | 7:ad4a19e26b84 | 174 | |
ClementBreteau | 7:ad4a19e26b84 | 175 | void Init_AX12() // Initialisation des différents paramétres |
ClementBreteau | 7:ad4a19e26b84 | 176 | { |
ClementBreteau | 7:ad4a19e26b84 | 177 | partieBasseCentre-> Set_Goal_speed(VITESSE); // VITESSE (0-1023) |
ClementBreteau | 7:ad4a19e26b84 | 178 | partieMainGauche-> Set_Goal_speed(VITESSE); |
ClementBreteau | 7:ad4a19e26b84 | 179 | partieMainDroite-> Set_Goal_speed(VITESSE); |
ClementBreteau | 7:ad4a19e26b84 | 180 | partiePoignetCentre-> Set_Goal_speed(VITESSE); |
ClementBreteau | 7:ad4a19e26b84 | 181 | |
ClementBreteau | 7:ad4a19e26b84 | 182 | partieBasseCentre-> Set_Mode(0); |
ClementBreteau | 7:ad4a19e26b84 | 183 | partieMainGauche-> Set_Mode(0); |
ClementBreteau | 7:ad4a19e26b84 | 184 | partieMainDroite-> Set_Mode(0); |
ClementBreteau | 7:ad4a19e26b84 | 185 | partiePoignetCentre-> Set_Mode(0); |
ClementBreteau | 7:ad4a19e26b84 | 186 | } |
ClementBreteau | 7:ad4a19e26b84 | 187 | |
ClementBreteau | 7:ad4a19e26b84 | 188 | |
ClementBreteau | 7:ad4a19e26b84 | 189 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 190 | /* FUNCTION NAME: Initialisation_position */ |
ClementBreteau | 7:ad4a19e26b84 | 191 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
ClementBreteau | 7:ad4a19e26b84 | 192 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 193 | void Initialisation_position(void){ |
ClementBreteau | 7:ad4a19e26b84 | 194 | multipleBrasCentral->multiple_goal_and_speed(4,TAB12); |
ClementBreteau | 7:ad4a19e26b84 | 195 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 196 | multipleBrasCentral->multiple_goal_and_speed(4,TAB1); |
ClementBreteau | 7:ad4a19e26b84 | 197 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 198 | } |
ClementBreteau | 7:ad4a19e26b84 | 199 | |
ClementBreteau | 7:ad4a19e26b84 | 200 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 201 | /* FUNCTION NAME: Preparation_prise */ |
ClementBreteau | 7:ad4a19e26b84 | 202 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
ClementBreteau | 7:ad4a19e26b84 | 203 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 204 | void Preparation_prise(void){ |
ClementBreteau | 7:ad4a19e26b84 | 205 | /*if (action_precedente == 0){ |
ClementBreteau | 7:ad4a19e26b84 | 206 | multipleBrasCentral->multiple_goal_and_speed(4,TAB12); |
ClementBreteau | 7:ad4a19e26b84 | 207 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 208 | action_precedente = 1; |
ClementBreteau | 7:ad4a19e26b84 | 209 | }*/ |
ClementBreteau | 7:ad4a19e26b84 | 210 | multipleBrasCentral->multiple_goal_and_speed(4,TAB2); |
ClementBreteau | 7:ad4a19e26b84 | 211 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 212 | } |
ClementBreteau | 7:ad4a19e26b84 | 213 | |
ClementBreteau | 7:ad4a19e26b84 | 214 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 215 | /* FUNCTION NAME: Stockage_haut */ |
ClementBreteau | 7:ad4a19e26b84 | 216 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
ClementBreteau | 7:ad4a19e26b84 | 217 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 218 | void Stockage_haut(void){ |
ClementBreteau | 7:ad4a19e26b84 | 219 | multipleBrasCentral->multiple_goal_and_speed(4,TAB3); |
ClementBreteau | 7:ad4a19e26b84 | 220 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 221 | multipleBrasCentral->multiple_goal_and_speed(4,TAB4); |
ClementBreteau | 7:ad4a19e26b84 | 222 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 223 | multipleBrasCentral->multiple_goal_and_speed(4,TAB5); |
ClementBreteau | 7:ad4a19e26b84 | 224 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 225 | multipleBrasCentral->multiple_goal_and_speed(4,TAB6); |
ClementBreteau | 7:ad4a19e26b84 | 226 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 227 | } |
ClementBreteau | 7:ad4a19e26b84 | 228 | |
ClementBreteau | 7:ad4a19e26b84 | 229 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 230 | /* FUNCTION NAME: Stockage_bas */ |
ClementBreteau | 7:ad4a19e26b84 | 231 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
ClementBreteau | 7:ad4a19e26b84 | 232 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 233 | void Stockage_bas(void){ |
ClementBreteau | 7:ad4a19e26b84 | 234 | multipleBrasCentral->multiple_goal_and_speed(4,TAB3); |
ClementBreteau | 7:ad4a19e26b84 | 235 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 236 | multipleBrasCentral->multiple_goal_and_speed(4,TAB7); |
ClementBreteau | 7:ad4a19e26b84 | 237 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 238 | } |
ClementBreteau | 7:ad4a19e26b84 | 239 | |
ClementBreteau | 7:ad4a19e26b84 | 240 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 241 | /* FUNCTION NAME: Deposer */ |
ClementBreteau | 7:ad4a19e26b84 | 242 | /* DESCRIPTION : Fonction qui permet de déposer le module */ |
ClementBreteau | 7:ad4a19e26b84 | 243 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 244 | void Deposer(void){ |
ClementBreteau | 7:ad4a19e26b84 | 245 | multipleBrasCentral->multiple_goal_and_speed(4,TAB9); |
ClementBreteau | 7:ad4a19e26b84 | 246 | wait(TIME/5); |
ClementBreteau | 7:ad4a19e26b84 | 247 | multipleBrasCentral->multiple_goal_and_speed(4,TAB8); |
ClementBreteau | 7:ad4a19e26b84 | 248 | wait(TIME/5); |
ClementBreteau | 7:ad4a19e26b84 | 249 | //multipleBrasCentral->multiple_goal_and_speed(4,TAB13); |
ClementBreteau | 7:ad4a19e26b84 | 250 | wait(TIME/5); |
ClementBreteau | 7:ad4a19e26b84 | 251 | } |
ClementBreteau | 7:ad4a19e26b84 | 252 | |
ClementBreteau | 7:ad4a19e26b84 | 253 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 254 | /* FUNCTION NAME: Preparation_depot_bas */ |
ClementBreteau | 7:ad4a19e26b84 | 255 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
ClementBreteau | 7:ad4a19e26b84 | 256 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 257 | void Preparation_depot_bas(void){ |
ClementBreteau | 7:ad4a19e26b84 | 258 | multipleBrasCentral->multiple_goal_and_speed(4,TAB8); |
ClementBreteau | 7:ad4a19e26b84 | 259 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 260 | } |
ClementBreteau | 7:ad4a19e26b84 | 261 | |
ClementBreteau | 7:ad4a19e26b84 | 262 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 263 | /* FUNCTION NAME: Preparation_depot_haut */ |
ClementBreteau | 7:ad4a19e26b84 | 264 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
ClementBreteau | 7:ad4a19e26b84 | 265 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 266 | void Preparation_depot_haut(void){ |
ClementBreteau | 7:ad4a19e26b84 | 267 | multipleBrasCentral->multiple_goal_and_speed(4,TAB14); |
ClementBreteau | 7:ad4a19e26b84 | 268 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 269 | multipleBrasCentral->multiple_goal_and_speed(4,TAB16); |
ClementBreteau | 7:ad4a19e26b84 | 270 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 271 | multipleBrasCentral->multiple_goal_and_speed(4,TAB15); |
ClementBreteau | 7:ad4a19e26b84 | 272 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 273 | multipleBrasCentral->multiple_goal_and_speed(4,TAB10); |
ClementBreteau | 7:ad4a19e26b84 | 274 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 275 | multipleBrasCentral->multiple_goal_and_speed(4,TAB8); |
ClementBreteau | 7:ad4a19e26b84 | 276 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 277 | } |
ClementBreteau | 7:ad4a19e26b84 | 278 | |
ClementBreteau | 7:ad4a19e26b84 | 279 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 280 | /* FUNCTION NAME: Pousser_module */ |
ClementBreteau | 7:ad4a19e26b84 | 281 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
ClementBreteau | 7:ad4a19e26b84 | 282 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 283 | void Pousser_module(void){ |
ClementBreteau | 7:ad4a19e26b84 | 284 | multipleBrasCentral->multiple_goal_and_speed(4,TAB11); |
ClementBreteau | 7:ad4a19e26b84 | 285 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 286 | } |
ClementBreteau | 7:ad4a19e26b84 | 287 | |
ClementBreteau | 7:ad4a19e26b84 | 288 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 289 | /* FUNCTION NAME: Initialisation_gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 290 | /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ |
ClementBreteau | 7:ad4a19e26b84 | 291 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 292 | void Initialisation_gauche(void){ |
ClementBreteau | 7:ad4a19e26b84 | 293 | //trois_myAX12_2->Set_Secure_Goal(235); |
ClementBreteau | 7:ad4a19e26b84 | 294 | multipleBrasGauche->multiple_goal_and_speed(4,TAB22); |
ClementBreteau | 7:ad4a19e26b84 | 295 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 296 | multipleBrasGauche->multiple_goal_and_speed(4,TAB21); |
ClementBreteau | 7:ad4a19e26b84 | 297 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 298 | } |
ClementBreteau | 7:ad4a19e26b84 | 299 | |
ClementBreteau | 7:ad4a19e26b84 | 300 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 301 | /* FUNCTION NAME: Preparation_prise_gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 302 | /* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */ |
ClementBreteau | 7:ad4a19e26b84 | 303 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 304 | void Preparation_prise_gauche(void){ |
ClementBreteau | 7:ad4a19e26b84 | 305 | //trois_myAX12_2->Set_Secure_Goal(120); |
ClementBreteau | 7:ad4a19e26b84 | 306 | } |
ClementBreteau | 7:ad4a19e26b84 | 307 | |
ClementBreteau | 7:ad4a19e26b84 | 308 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 309 | /* FUNCTION NAME: Prendre_module_gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 310 | /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 311 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 312 | void Prendre_module_gauche(void){ |
ClementBreteau | 7:ad4a19e26b84 | 313 | //trois_myAX12_2->Set_Secure_Goal(160); |
ClementBreteau | 7:ad4a19e26b84 | 314 | } |
ClementBreteau | 7:ad4a19e26b84 | 315 | |
ClementBreteau | 7:ad4a19e26b84 | 316 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 317 | /* FUNCTION NAME: Preparatio_module_gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 318 | /* DESCRIPTION : Fonction qui prepare le tournante */ |
ClementBreteau | 7:ad4a19e26b84 | 319 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 320 | void Preparation_module_gauche(void){ |
ClementBreteau | 7:ad4a19e26b84 | 321 | multipleBrasGauche->multiple_goal_and_speed(4,TAB22); |
ClementBreteau | 7:ad4a19e26b84 | 322 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 323 | } |
ClementBreteau | 7:ad4a19e26b84 | 324 | |
ClementBreteau | 7:ad4a19e26b84 | 325 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 326 | /* FUNCTION NAME: Tourner_module_gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 327 | /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ |
ClementBreteau | 7:ad4a19e26b84 | 328 | /****************************************************************************************/ |
ClementBreteau | 7:ad4a19e26b84 | 329 | void Tourner_module_gauche(void){ |
ClementBreteau | 7:ad4a19e26b84 | 330 | multipleBrasGauche->multiple_goal_and_speed(4,TAB23); |
ClementBreteau | 7:ad4a19e26b84 | 331 | wait(TIME); |
ClementBreteau | 7:ad4a19e26b84 | 332 | } |
ClementBreteau | 7:ad4a19e26b84 | 333 | |
ClementBreteau | 7:ad4a19e26b84 | 334 | void getPosiotionCentrale(void){ |
ClementBreteau | 7:ad4a19e26b84 | 335 | //AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, |
ClementBreteau | 7:ad4a19e26b84 | 336 | led = 1; |
ClementBreteau | 7:ad4a19e26b84 | 337 | pc.printf("\n\r * pince avant * \n\r"); |
ClementBreteau | 7:ad4a19e26b84 | 338 | |
ClementBreteau | 7:ad4a19e26b84 | 339 | pc.printf("base : %lf \n\r ", partieBasseCentre->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 340 | pc.printf("coude : %lf \n\r ", partiePoignetCentre->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 341 | pc.printf("doigtG: %lf \n\r ", partieMainGaucheCentre->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 342 | pc.printf("doigtD: %lf \n\n\r ", partieMainDroiteCentre->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 343 | |
ClementBreteau | 7:ad4a19e26b84 | 344 | wait(0.2); |
ClementBreteau | 7:ad4a19e26b84 | 345 | led = 0; |
ClementBreteau | 7:ad4a19e26b84 | 346 | |
ClementBreteau | 7:ad4a19e26b84 | 347 | } |
ClementBreteau | 7:ad4a19e26b84 | 348 | |
ClementBreteau | 7:ad4a19e26b84 | 349 | void getPosiotionGauche(void){ |
ClementBreteau | 7:ad4a19e26b84 | 350 | //AX12 *partieBasseGauche, *partieMainGauche |
ClementBreteau | 7:ad4a19e26b84 | 351 | led = 1; |
ClementBreteau | 7:ad4a19e26b84 | 352 | pc.printf("\n\r * bras gauche *\n\r"); |
ClementBreteau | 7:ad4a19e26b84 | 353 | |
ClementBreteau | 7:ad4a19e26b84 | 354 | pc.printf("epaule : %lf \n\r ",partieBasseGauche->Get_Position()); |
ClementBreteau | 7:ad4a19e26b84 | 355 | pc.printf("main : %lf \n\r ",partieMainGauche->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 356 | pc.printf("porte : %lf \n\r ",partiePorteGauche->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 357 | |
ClementBreteau | 7:ad4a19e26b84 | 358 | wait(0.2); |
ClementBreteau | 7:ad4a19e26b84 | 359 | led = 0; |
ClementBreteau | 7:ad4a19e26b84 | 360 | |
ClementBreteau | 7:ad4a19e26b84 | 361 | } |
ClementBreteau | 7:ad4a19e26b84 | 362 | |
ClementBreteau | 7:ad4a19e26b84 | 363 | void getPosiotionDroite(void){ |
ClementBreteau | 7:ad4a19e26b84 | 364 | //AX12 *partieBasseDroite, *partieMainDroite, *multipleBrasDroite; |
ClementBreteau | 7:ad4a19e26b84 | 365 | led = 1; |
ClementBreteau | 7:ad4a19e26b84 | 366 | pc.printf("\n\r * bras droit *\n\r"); |
ClementBreteau | 7:ad4a19e26b84 | 367 | |
ClementBreteau | 7:ad4a19e26b84 | 368 | pc.printf("epaule : %lf \n\r ",partieBasseDroite->Get_Position()); |
ClementBreteau | 7:ad4a19e26b84 | 369 | pc.printf("main : %lf \n\r ",partieMainDroite->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 370 | pc.printf("porte : %lf \n\r ",partiePorteDroite->Get_Position() ); |
ClementBreteau | 7:ad4a19e26b84 | 371 | |
ClementBreteau | 7:ad4a19e26b84 | 372 | |
ClementBreteau | 7:ad4a19e26b84 | 373 | wait(0.2); |
ClementBreteau | 7:ad4a19e26b84 | 374 | led = 0; |
ClementBreteau | 7:ad4a19e26b84 | 375 | |
ClementBreteau | 7:ad4a19e26b84 | 376 | } |