Strategie_small.cpp

00001 #include "StrategieManager.h"
00002 #ifdef ROBOT_SMALL
00003 #include "Config_small.h"
00004 
00005 unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
00006 //unsigned short telemetreDistance;
00007 
00008 
00009 /****************************************************************************************/
00010 /* FUNCTION NAME: doFunnyAction                                                         */
00011 /* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
00012 /****************************************************************************************/
00013 void doFunnyAction(void) {
00014     
00015     
00016 }
00017 //L'angle est entre 0 et 1023
00018 void XL320_setGoal(unsigned char id, unsigned short angle);
00019 
00020 void XL320_setGoal(unsigned char id, unsigned short angle)
00021 {
00022     CANMessage msgTx=CANMessage();
00023     msgTx.id=SERVO_XL320;
00024     msgTx.len=3;
00025     msgTx.format=CANStandard;
00026     msgTx.type=CANData;
00027     msgTx.data[0]=(unsigned char)id;
00028     // from sur 2 octets
00029     msgTx.data[1]=(unsigned char)angle;
00030     msgTx.data[2]=(unsigned char)(angle>>8);
00031 
00032     can1.write(msgTx);
00033 }
00034 
00035 
00036 /****************************************************************************************/
00037 /* FUNCTION NAME: doAction                                                              */
00038 /* DESCRIPTION  : Effectuer une action specifique                                       */
00039 /****************************************************************************************/
00040 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
00041     int retour = 1;
00042     switch(id) {
00043         case 100: // preparation prise bras central
00044             AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_PRISE);
00045         break;
00046         case 101:// stockage haut bras central
00047             AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_HAUT);
00048         break;
00049         case 102:// stockage bas bras central
00050             AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_BAS);
00051         break;
00052         case 103:// ouvrir la main du bras cental 
00053             AX12_automate(AX12_PINCE_CENTRALE_DEPOSER);
00054         break;
00055         case 104:// preparation de depot du module bas du bras central
00056             AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
00057         break;
00058         case 105:// preparation de depot du module haut du bras central
00059             AX12_automate(AX12_PINCE_CENTRALE_DEPOT_HAUT);
00060             AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
00061         break;
00062         case 106:// on rentre la pince dans le robot
00063             AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
00064         break;
00065         case 107: // on ferme la pince du robot
00066             AX12_automate(AX12_PINCE_CENTRALE_PRISE_MODULE);
00067         break;
00068         
00069         case 110:// ouvrir une porte avant
00070             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
00071                 if(speed == 1) speed = 0;
00072                 else speed = 1;
00073             }
00074             
00075             if (speed == 1){
00076                 AX12_automate(AX12_DROIT_CROC_OUVERT);
00077             }else{
00078                 AX12_automate(AX12_GAUCHE_CROC_OUVERT);    
00079             }
00080         break;
00081         case 111:// securiser un module avec une porte avant
00082             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
00083                 if(speed == 1) speed = 0;
00084                 else speed = 1;
00085             }
00086             
00087             if (speed == 1){
00088                 AX12_automate(AX12_DROIT_CROC_FERME);
00089             }else{
00090                 AX12_automate(AX12_GAUCHE_CROC_FERME);    
00091             }
00092         break;
00093         case 112:// ranger le porte avant dans le robot
00094             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
00095                 if(speed == 1) speed = 0;
00096                 else speed = 1;
00097             }
00098             
00099             if (speed == 1){
00100                 AX12_automate(AX12_DROIT_CROC_INITIALE);
00101             }else{
00102                 AX12_automate(AX12_GAUCHE_CROC_INITIALE);    
00103             }
00104         break;
00105         case 120:// poser une main tourneuse
00106             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
00107                 if(speed == 2) speed = 1;
00108                 else speed = 2;
00109                 if(angle == 1) angle = 0;
00110                 else angle = 1;
00111             }
00112             
00113             if (speed == 1){
00114                 AX12_automate(AX12_TOURNANTE_DROIT_PREPARATION);
00115             }else{
00116                 AX12_automate(AX12_TOURNANTE_GAUCHE_PREPARATION);    
00117             }
00118         break;
00119         
00120         case 121:// relever une main tourneuse
00121             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
00122                 if(speed == 2) speed = 1;
00123                 else speed = 2;
00124                 if(angle == 1) angle = 0;
00125                 else angle = 1;
00126             }
00127             
00128             if (speed == 1){
00129                 AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
00130             }else{
00131                 AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);    
00132             }
00133             
00134         break;
00135         
00136         case 122:// tourner un module
00137             if(InversStrat == 1) {//Si c'est inversé
00138                 if(speed == 2) speed = 1;
00139                 else speed = 2;
00140                 if(angle == 1) angle = 0;
00141                 else angle = 1;
00142             }
00143             
00144             if (speed == 1){
00145                 AX12_automate(AX12_TOURNANTE_DROIT_MODULE);
00146                 Tourner_module_droit();
00147             }else{
00148                 AX12_automate(AX12_TOURNANTE_GAUCHE_MODULE);    
00149                 Tourner_module_gauche();
00150             }
00151         break;
00152         
00153         case 200 :
00154             telemetreDistance = dataTelemetre();
00155             wait_ms(1);
00156             telemetreDistance = dataTelemetre();
00157             telemetreDistance = telemetreDistance - 170;
00158         break;
00159         
00160         case 201 :
00161             SendRawId(0x96);
00162             retour = 2;
00163         break;
00164         
00165         case 10://Désactiver le stop
00166             isStopEnable = 0;
00167         break;
00168         case 11://Activer le stop
00169             isStopEnable = 1;
00170         break;
00171         case 20://Désactiver l'asservissement
00172             setAsservissementEtat(0);
00173         break;
00174         case 21://Activer l'asservissement
00175             setAsservissementEtat(1);
00176         break; 
00177         
00178         case 22://Changer la vitesse du robot
00179             SendSpeed(speed,(unsigned short)angle);
00180             wait_us(200);
00181         break;
00182         
00183         case 30://Action tempo
00184             wait_ms(speed);
00185         break;
00186         
00187         default:
00188             retour = 0;//L'action n'existe pas, il faut utiliser le CAN
00189         
00190     }
00191     return retour;//L'action est spécifique.
00192     
00193 }
00194 
00195 /****************************************************************************************/
00196 /* FUNCTION NAME: initRobot                                                             */
00197 /* DESCRIPTION  : initialiser le robot                                                  */
00198 /****************************************************************************************/
00199 void initRobot(void) {
00200     /**
00201     On enregistre les id des AX12 présent sur la carte
00202     **/
00203     /*AX12_register(1,AX12_SERIAL1,0x0FF);
00204     AX12_register(2,AX12_SERIAL1);
00205     AX12_register(18,AX12_SERIAL1);
00206     AX12_register(4,AX12_SERIAL2);
00207     AX12_register(16,AX12_SERIAL2);
00208     AX12_register(17,AX12_SERIAL2,0x0FF);*/
00209     
00210     //runRobotTest();
00211     
00212     /*
00213     AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
00214     AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
00215     AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF);
00216     AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
00217     AX12_processChange();*/
00218     
00219     initialisation_AX12();
00220     
00221 }
00222 
00223 /****************************************************************************************/
00224 /* FUNCTION NAME: initRobotActionneur                                                   */
00225 /* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
00226 /****************************************************************************************/
00227 void initRobotActionneur(void)
00228 {
00229     moteurGauchePWM(0);
00230     moteurDroitPWM(0);
00231     AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
00232     AX12_automate(AX12_GAUCHE_CROC_INITIALE);
00233     AX12_automate(AX12_DROIT_CROC_INITIALE);
00234     AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
00235     AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
00236 }
00237 
00238 /****************************************************************************************/
00239 /* FUNCTION NAME: runTest                                                               */
00240 /* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
00241 /****************************************************************************************/
00242 void runRobotTest(void) 
00243 {
00244 
00245 }
00246 
00247 /****************************************************************************************/
00248 /* FUNCTION NAME: SelectStrategy                                                        */
00249 /* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
00250 /* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */
00251 /****************************************************************************************/
00252 int SelectStrategy(unsigned char id)
00253 {
00254     
00255     switch(id)
00256     {
00257         case 1:
00258             strcpy(cheminFileStart,"/local/strat1.txt");
00259             return FileExists(cheminFileStart);
00260         case 2:
00261             strcpy(cheminFileStart,"/local/strat2.txt");
00262             return FileExists(cheminFileStart);
00263         case 3:
00264             strcpy(cheminFileStart,"/local/strat3.txt");
00265             return FileExists(cheminFileStart);
00266         case 4:
00267             strcpy(cheminFileStart,"/local/strat4.txt");
00268             return FileExists(cheminFileStart);
00269         case 5:
00270             strcpy(cheminFileStart,"/local/strat5.txt");
00271             return FileExists(cheminFileStart);
00272         case 6:
00273             strcpy(cheminFileStart,"/local/strat6.txt");
00274             return FileExists(cheminFileStart);
00275         case 7:
00276             strcpy(cheminFileStart,"/local/strat7.txt");
00277             return FileExists(cheminFileStart);
00278         case 8:
00279             strcpy(cheminFileStart,"/local/strat8.txt");
00280             return FileExists(cheminFileStart);
00281         case 9:
00282             strcpy(cheminFileStart,"/local/strat9.txt");
00283             return FileExists(cheminFileStart);
00284         case 10:
00285             strcpy(cheminFileStart,"/local/strat10.txt");
00286             return FileExists(cheminFileStart);
00287         case 11:
00288             strcpy(cheminFileStart,"/local/grand_8.txt");
00289             return FileExists(cheminFileStart);
00290         
00291         case 0x10:
00292             strcpy(cheminFileStart,"/local/demoBras.txt");
00293             return FileExists(cheminFileStart);
00294         
00295         default:
00296             strcpy(cheminFileStart,"/local/strat1.txt");
00297             SendRawId(0x258);
00298             return 0;
00299     }
00300 }
00301 
00302 /****************************************************************************************/
00303 /* FUNCTION NAME: needToStop                                                            */
00304 /* DESCRIPTION  : Savoir si il faut autoriser le stop du robot via balise               */
00305 /****************************************************************************************/
00306 unsigned char needToStop(void)
00307 {
00308     return isStopEnable;
00309 }
00310 
00311 /****************************************************************************************/
00312 /* FUNCTION NAME: doBeforeEndAction                                                     */
00313 /* DESCRIPTION  : Terminer les actions du robot 1s avant la fin du match                */
00314 /****************************************************************************************/
00315 void doBeforeEndAction(void)
00316 {
00317     
00318 }
00319 
00320 #endif