CRAC Team / command_AX12_petit_robot_V4tempo

code ax12 petit robot 12/05/2017

Fork of command_AX12_petit_robot_V3 by CRAC Team

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?

UserRevisionLine numberNew 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 }