CRAC Team / Mbed 2 deprecated carte_esclave201_petit_rob

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Actionneurs/Actionneurs.cpp@11:6c11c081c855, 2019-05-20 (annotated)

Committer:
Artiom
Date:
Mon May 20 11:38:43 2019 +0000
Revision:
11:6c11c081c855
Parent:
7:553f3f1c2c53
Child:
12:191486ba5118
ajout de la fonction verification fin de mouvement+ack ventouse relache

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:bc74da1c502f 1 #include "Actionneurs.h"
Artiom 0:bc74da1c502f 2 #include "ident_crac.h"
Artiom 0:bc74da1c502f 3 #include "mbed.h"
Artiom 0:bc74da1c502f 4 #include "fonctions_herkulex.h"
Artiom 1:568955af8c2b 5 #include "main.h"
Artiom 0:bc74da1c502f 6
Artiom 0:bc74da1c502f 7 #ifdef ROBOT_SMALL
Artiom 2:9e63099cca99 8 void gabarit_petit_robot(void)
Artiom 0:bc74da1c502f 9 {
Artiom 0:bc74da1c502f 10
Artiom 0:bc74da1c502f 11 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:bc74da1c502f 12 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:bc74da1c502f 13 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:bc74da1c502f 14
Artiom 0:bc74da1c502f 15 uint16_t pos_av_gauche[2] = {520,780};//epaule,poigne520,780
Artiom 3:e2336813297b 16 uint16_t pos_av_centre[2] = {550,600};//470,350
Artiom 0:bc74da1c502f 17 uint16_t pos_av_droite[2] = {550,270};
Artiom 0:bc74da1c502f 18
Artiom 0:bc74da1c502f 19 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:bc74da1c502f 20 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:bc74da1c502f 21 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:bc74da1c502f 22
Artiom 0:bc74da1c502f 23 uint16_t pos_ar_gauche[2] = {530,780};
Artiom 3:e2336813297b 24 uint16_t pos_ar_centre[2] = {600,500};//500,250
Artiom 0:bc74da1c502f 25 uint16_t pos_ar_droite[2] = {430,200};
Artiom 0:bc74da1c502f 26
Artiom 1:568955af8c2b 27
Artiom 1:568955af8c2b 28 uint8_t servos_sol[4] = {GLED_ON, AR_sol, GLED_ON, AV_sol};
Artiom 4:4a79942713fa 29 uint16_t pos_sol[2] = {230,750}; //200 ,800 (90°)
Artiom 1:568955af8c2b 30
Artiom 0:bc74da1c502f 31 int speed=100;
Artiom 1:568955af8c2b 32
Artiom 0:bc74da1c502f 33 deverouillage_torque();
Artiom 0:bc74da1c502f 34 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 0:bc74da1c502f 35 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 0:bc74da1c502f 36 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 0:bc74da1c502f 37
Artiom 0:bc74da1c502f 38 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:bc74da1c502f 39 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:bc74da1c502f 40 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 1:568955af8c2b 41
Artiom 1:568955af8c2b 42 positionControl_Mul_ensemble_complex(2,speed,servos_sol, pos_sol,4);
Artiom 0:bc74da1c502f 43 }
Artiom 1:568955af8c2b 44
Artiom 0:bc74da1c502f 45 void presentoir_avant(void)
Artiom 0:bc74da1c502f 46 {
Artiom 0:bc74da1c502f 47 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:bc74da1c502f 48 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:bc74da1c502f 49 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:bc74da1c502f 50
Artiom 0:bc74da1c502f 51 uint16_t pos_av_gauche[2] = {520,490};
Artiom 0:bc74da1c502f 52 uint16_t pos_av_centre[2] = {470,512};
Artiom 0:bc74da1c502f 53 uint16_t pos_av_droite[2] = {550,540};
Artiom 0:bc74da1c502f 54
Artiom 2:9e63099cca99 55 int speed=1;
Artiom 0:bc74da1c502f 56
Artiom 2:9e63099cca99 57 //deverouillage_torque();
Artiom 0:bc74da1c502f 58 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 0:bc74da1c502f 59 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 0:bc74da1c502f 60 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 2:9e63099cca99 61
Artiom 1:568955af8c2b 62 can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AV));
Artiom 1:568955af8c2b 63 }
Artiom 0:bc74da1c502f 64
Artiom 0:bc74da1c502f 65 void presentoir_arriere(void)
Artiom 0:bc74da1c502f 66 {
Artiom 0:bc74da1c502f 67 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:bc74da1c502f 68 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:bc74da1c502f 69 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:bc74da1c502f 70
Artiom 0:bc74da1c502f 71 uint16_t pos_ar_gauche[2] = {530,512};
Artiom 0:bc74da1c502f 72 uint16_t pos_ar_centre[2] = {500,430};
Artiom 0:bc74da1c502f 73 uint16_t pos_ar_droite[2] = {430,470};
Artiom 0:bc74da1c502f 74
Artiom 2:9e63099cca99 75 int speed=1;
Artiom 0:bc74da1c502f 76
Artiom 2:9e63099cca99 77 //deverouillage_torque();
Artiom 0:bc74da1c502f 78 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:bc74da1c502f 79 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:bc74da1c502f 80 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 0:bc74da1c502f 81
Artiom 1:568955af8c2b 82 can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AR));
Artiom 1:568955af8c2b 83
Artiom 0:bc74da1c502f 84 }
Artiom 1:568955af8c2b 85
Artiom 0:bc74da1c502f 86 void balance_avant(void)
Artiom 0:bc74da1c502f 87 {
Artiom 11:6c11c081c855 88 /*
Artiom 11:6c11c081c855 89 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 11:6c11c081c855 90 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 11:6c11c081c855 91 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 11:6c11c081c855 92
Artiom 11:6c11c081c855 93 uint16_t pos_av_gauche[2] = {800,780};
Artiom 11:6c11c081c855 94 uint16_t pos_av_centre[2] = {200,220};
Artiom 11:6c11c081c855 95 uint16_t pos_av_droite[2] = {250,270};
Artiom 11:6c11c081c855 96
Artiom 11:6c11c081c855 97 int speed=25;
Artiom 11:6c11c081c855 98
Artiom 11:6c11c081c855 99 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 11:6c11c081c855 100 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 11:6c11c081c855 101 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);*/
Artiom 0:bc74da1c502f 102 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:bc74da1c502f 103 uint16_t pos_av_droite[2] = {250,270};
Artiom 11:6c11c081c855 104 int speed=1;
Artiom 11:6c11c081c855 105 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 11:6c11c081c855 106 verification();
Artiom 11:6c11c081c855 107
Artiom 11:6c11c081c855 108 char pompe=AV_DROIT;
Artiom 11:6c11c081c855 109 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 0:bc74da1c502f 110
Artiom 0:bc74da1c502f 111
Artiom 0:bc74da1c502f 112 }
Artiom 0:bc74da1c502f 113 void balance_arriere(void)
Artiom 0:bc74da1c502f 114 {
Artiom 0:bc74da1c502f 115 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:bc74da1c502f 116 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:bc74da1c502f 117 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:bc74da1c502f 118
Artiom 0:bc74da1c502f 119 uint16_t pos_ar_gauche[2] = {820,780};
Artiom 0:bc74da1c502f 120 uint16_t pos_ar_centre[2] = {230,150};
Artiom 0:bc74da1c502f 121 uint16_t pos_ar_droite[2] = {150,200};
Artiom 0:bc74da1c502f 122
Artiom 1:568955af8c2b 123 int speed=25;
Artiom 0:bc74da1c502f 124
Artiom 0:bc74da1c502f 125 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:bc74da1c502f 126 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:bc74da1c502f 127 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 0:bc74da1c502f 128
Artiom 0:bc74da1c502f 129 }
Artiom 0:bc74da1c502f 130
Artiom 1:568955af8c2b 131 void goldenium_avant(void)
Artiom 1:568955af8c2b 132 {
Artiom 1:568955af8c2b 133 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 1:568955af8c2b 134 uint16_t pos_av_centre[2] = {200,220};
Artiom 1:568955af8c2b 135 int speed=25;
Artiom 1:568955af8c2b 136
Artiom 1:568955af8c2b 137 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 1:568955af8c2b 138
Artiom 1:568955af8c2b 139 char pompe=AV_CENTRE;
Artiom 1:568955af8c2b 140 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 2:9e63099cca99 141
Artiom 1:568955af8c2b 142 }
Artiom 1:568955af8c2b 143
Artiom 1:568955af8c2b 144 void goldenium_arriere(void)
Artiom 1:568955af8c2b 145 {
Artiom 1:568955af8c2b 146 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 1:568955af8c2b 147 uint16_t pos_ar_centre[2] = {230,150};
Artiom 1:568955af8c2b 148 int speed=25;
Artiom 2:9e63099cca99 149
Artiom 1:568955af8c2b 150 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 1:568955af8c2b 151
Artiom 1:568955af8c2b 152 char pompe=AR_CENTRE;
Artiom 1:568955af8c2b 153 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 2:9e63099cca99 154
Artiom 1:568955af8c2b 155 }
Artiom 1:568955af8c2b 156
Artiom 1:568955af8c2b 157 void accelerateur_avant(void)
Artiom 1:568955af8c2b 158 {
Artiom 1:568955af8c2b 159 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 1:568955af8c2b 160 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 1:568955af8c2b 161 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 1:568955af8c2b 162
Artiom 1:568955af8c2b 163 uint16_t pos_av_gauche[2] = {800,490};
Artiom 1:568955af8c2b 164 uint16_t pos_av_centre[2] = {200,220};
Artiom 1:568955af8c2b 165 uint16_t pos_av_droite[2] = {250,540};
Artiom 1:568955af8c2b 166
Artiom 1:568955af8c2b 167 int speed=25;
Artiom 1:568955af8c2b 168
Artiom 1:568955af8c2b 169 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 1:568955af8c2b 170 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 1:568955af8c2b 171 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 6:45f9cf44718a 172 /*
Artiom 6:45f9cf44718a 173 for(char pompe=1; pompe<=3; pompe++) {
Artiom 6:45f9cf44718a 174 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 6:45f9cf44718a 175 }*/
Artiom 2:9e63099cca99 176 can.write(CANMessage(HACHEUR_RELEASE_AV));
Artiom 1:568955af8c2b 177 }
Artiom 1:568955af8c2b 178 void accelerateur_arriere(void)
Artiom 1:568955af8c2b 179 {
Artiom 1:568955af8c2b 180 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 1:568955af8c2b 181 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 1:568955af8c2b 182 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 1:568955af8c2b 183
Artiom 1:568955af8c2b 184 uint16_t pos_ar_gauche[2] = {820,512};
Artiom 1:568955af8c2b 185 uint16_t pos_ar_centre[2] = {230,150};
Artiom 1:568955af8c2b 186 uint16_t pos_ar_droite[2] = {150,470};
Artiom 1:568955af8c2b 187
Artiom 1:568955af8c2b 188 int speed=25;
Artiom 1:568955af8c2b 189
Artiom 1:568955af8c2b 190 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 1:568955af8c2b 191 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 1:568955af8c2b 192 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 6:45f9cf44718a 193 /*
Artiom 6:45f9cf44718a 194 for(char pompe=5; pompe<=7; pompe++) {
Artiom 6:45f9cf44718a 195 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 6:45f9cf44718a 196 }*/
Artiom 6:45f9cf44718a 197 can.write(CANMessage(HACHEUR_RELEASE_AR));
Artiom 2:9e63099cca99 198
Artiom 1:568955af8c2b 199 }
Artiom 1:568955af8c2b 200
Artiom 1:568955af8c2b 201
Artiom 1:568955af8c2b 202
Artiom 6:45f9cf44718a 203 void sol_avant_baisser(void)
Artiom 1:568955af8c2b 204 {
Artiom 1:568955af8c2b 205 int speed=1;
Artiom 1:568955af8c2b 206 char pompe=AV_BAS;
Artiom 1:568955af8c2b 207
Artiom 1:568955af8c2b 208 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 11:6c11c081c855 209 positionControl(AV_sol,480,speed,BLED_ON,4);//descend
Artiom 6:45f9cf44718a 210 }
Artiom 6:45f9cf44718a 211 void sol_avant_remonter(void)
Artiom 6:45f9cf44718a 212 {
Artiom 6:45f9cf44718a 213 int speed=1;
Artiom 1:568955af8c2b 214 positionControl(AV_sol,800,speed,BLED_ON,4);//remonte
Artiom 1:568955af8c2b 215 }
Artiom 1:568955af8c2b 216
Artiom 6:45f9cf44718a 217
Artiom 6:45f9cf44718a 218
Artiom 6:45f9cf44718a 219 void sol_arriere_baisser(void)
Artiom 1:568955af8c2b 220 {
Artiom 1:568955af8c2b 221 int speed=1;
Artiom 1:568955af8c2b 222 char pompe=AR_BAS;
Artiom 1:568955af8c2b 223
Artiom 1:568955af8c2b 224 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 11:6c11c081c855 225 positionControl(AR_sol,535,speed,BLED_ON,4);//descend
Artiom 6:45f9cf44718a 226 }
Artiom 6:45f9cf44718a 227 void sol_arriere_remonter(void)
Artiom 6:45f9cf44718a 228 {
Artiom 6:45f9cf44718a 229 int speed=1;
Artiom 1:568955af8c2b 230 positionControl(AR_sol,200,speed,BLED_ON,4);//remonte
Artiom 2:9e63099cca99 231 }
Artiom 2:9e63099cca99 232
Artiom 2:9e63099cca99 233
Artiom 2:9e63099cca99 234 void sol_avant_relache(void)
Artiom 2:9e63099cca99 235 {
Artiom 2:9e63099cca99 236 int speed=1;
Artiom 2:9e63099cca99 237 char pompe=AV_BAS;
Artiom 2:9e63099cca99 238
Artiom 2:9e63099cca99 239 positionControl(AV_sol,512,speed,BLED_ON,4);//descend
Artiom 1:568955af8c2b 240 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 2:9e63099cca99 241
Artiom 7:553f3f1c2c53 242 wait(0.5);//faire automate dans main
Artiom 2:9e63099cca99 243
Artiom 2:9e63099cca99 244 positionControl(AV_sol,800,speed,BLED_ON,4);//remonte
Artiom 2:9e63099cca99 245 }
Artiom 2:9e63099cca99 246
Artiom 2:9e63099cca99 247 void sol_arriere_relache(void)
Artiom 2:9e63099cca99 248 {
Artiom 2:9e63099cca99 249 int speed=1;
Artiom 2:9e63099cca99 250 char pompe=AR_BAS;
Artiom 2:9e63099cca99 251
Artiom 2:9e63099cca99 252 positionControl(AR_sol,512,speed,BLED_ON,4);//descend
Artiom 2:9e63099cca99 253 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 2:9e63099cca99 254
Artiom 7:553f3f1c2c53 255 wait(0.5);//faire automate dans main
Artiom 2:9e63099cca99 256
Artiom 2:9e63099cca99 257 positionControl(AR_sol,200,speed,BLED_ON,4);//remonte
Artiom 1:568955af8c2b 258 }
Artiom 1:568955af8c2b 259
Artiom 7:553f3f1c2c53 260 #endif
Artiom 7:553f3f1c2c53 261
Artiom 7:553f3f1c2c53 262 #ifdef ROBOT_BIG
Artiom 7:553f3f1c2c53 263
Artiom 1:568955af8c2b 264
Artiom 1:568955af8c2b 265
Artiom 1:568955af8c2b 266
Artiom 0:bc74da1c502f 267
Artiom 7:553f3f1c2c53 268 #endif
Artiom 7:553f3f1c2c53 269
Artiom 7:553f3f1c2c53 270