File content as of revision 28:acd18776ed2d:

#include "StrategieManager.h"
#ifdef ROBOT_BIG
#include "Config_big.h"

unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises

/****************************************************************************************/
/* FUNCTION NAME: doFunnyAction                                                         */
/* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
/****************************************************************************************/
void doFunnyAction(void) {
    //envoie de la funny action
    // 0x007, 01, 01
        CANMessage msgTx=CANMessage();
        msgTx.id=GLOBAL_FUNNY_ACTION;
        msgTx.format=CANStandard;
        msgTx.type=CANData;
        msgTx.len=2;
        msgTx.data[0]=0x01;
        msgTx.data[1]=0x01;
        can1.write(msgTx);
}

/****************************************************************************************/
/* FUNCTION NAME: doAction                                                              */
/* DESCRIPTION  : Effectuer une action specifique                                       */
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
    CANMessage msgTx=CANMessage();
    int localData = 0, localData2;
    switch(id) {
        case 100: // position initiale
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=1;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 101:  /// preparation prise
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=2;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 102: // stockage haut
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=3;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 103: // stockage bas
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=4;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 104: // pousser module
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=7;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 105: // preparation prise
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=6;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 106: //deposer module
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
                }
            
            msgTx.data[0]=5;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 110: // repos bras avant
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_AVANT; 
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x10;
            else localData = 0x24;
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 111: // init bras module
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_AVANT;
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x0A;
            else localData = 0x1E;
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 112: // bouger module avant la gouttière
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_AVANT;
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                    else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x0B;
            else localData = 0x1F;
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 113: // deposer module
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            localData2 = BRAS_AVANT;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
                else localData2 = BRAS_AVANT;
            }
            
            if(localData2 == BRAS_AVANT){
                if(speed == INV){
                        localData = 0x0E;
                    }else{
                        localData = 0x0C;
                    }
            }else{
                if(speed == INV){
                        localData = 0x22;
                    }else{
                        localData = 0x20;
                    }
            }
            
            msgTx.data[0]=localData;
            msgTx.data[1]=localData2;
            can1.write(msgTx);
        break;
        
        case 114: // repositionner bras avant
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_AVANT;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x0F;
            else localData = 0x23; 
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 115: // pompe avant ON
            msgTx.id=0x400;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            localData = BRAS_AVANT;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
                else localData = BRAS_AVANT;
            } 
            
            msgTx.data[0]=1;
            msgTx.data[1]=localData;
            
            can1.write(msgTx);
            wait_ms(1);
            can1.write(msgTx);
        break;
        
        case 116: // pompe avant OFF
            msgTx.id=0x400;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            localData = BRAS_AVANT;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
                else localData = BRAS_AVANT;
            } 
            
            msgTx.data[0]=0;
            msgTx.data[1]=localData;
            
            can1.write(msgTx);
            wait_ms(1);
            can1.write(msgTx);
        break;
        
        case 120: // repos bras arrière
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_ARRIERE;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x10;
            else localData = 0x24; 
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 121: // position d'init avant de prendre un module bras arriere
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_ARRIERE;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x0A;
            else localData = 0x1E; 
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 122: // bouger le bras arriere avant la gouttiere
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_ARRIERE;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x0B;
            else localData = 0x1F; 
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 123: // deposer module
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            localData2 = BRAS_ARRIERE;
            
            if (InversStrat){ // si on est inversé, on echange les bras
                if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
                else localData2 = BRAS_AVANT;
            }
            
            if(localData2 == BRAS_AVANT){
                if(speed == INV){
                        localData = 0x0E;
                    }else{
                        localData = 0x0C;
                    }
            }else{
                if(speed == INV){
                        localData = 0x22;
                    }else{
                        localData = 0x20;
                    }
            }
            
            msgTx.data[0]=localData;
            msgTx.data[1]=localData2;
            can1.write(msgTx);
        break;
        
        case 124: // remettre le bras arriere en posiion initiale
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            speed = BRAS_ARRIERE;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
                else speed = BRAS_AVANT;
            }
            if (speed == BRAS_AVANT)localData = 0x0F;
            else localData = 0x23; 
            
            msgTx.data[0]=localData;
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        
        case 125: // pompe pwm arriere ON
            msgTx.id=0x400;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            localData = BRAS_ARRIERE;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
                else localData = BRAS_AVANT;
            } 
            
            msgTx.data[0]=1;
            msgTx.data[1]=localData;
            
            can1.write(msgTx);
            wait_ms(1);
            can1.write(msgTx);
        break;
        
        case 126: // pompe arriere pwm OFF
            msgTx.id=0x400;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            localData = BRAS_ARRIERE;
            if (InversStrat){ // si on est inversé, on echange les bras
                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
                else localData = BRAS_AVANT;
            } 
            
            msgTx.data[0]=0;
            msgTx.data[1]=localData;
            
            can1.write(msgTx);
            wait_ms(1);
            can1.write(msgTx);
        break;
        
        case 130: // baiser bras turbine
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            msgTx.data[0]=0x14;
            msgTx.data[1]=BRAS_AVANT;
            can1.write(msgTx);
        break;
        
        case 131: // lever bras turbine
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            msgTx.data[0]=0x15;
            msgTx.data[1]=BRAS_AVANT;
            can1.write(msgTx);
        break;
        
        case 132: // allumer turbine
            msgTx.id=0x403;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            msgTx.data[0]=1;
            msgTx.data[1]=BRAS_AVANT;
            can1.write(msgTx);
            waitingAckFrom = 0;
            waitingAckID = 0;
        break;
        
        case 133: // stop turbine
            msgTx.id=0x403;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            msgTx.data[0]=0;
            msgTx.data[1]=BRAS_AVANT;
            can1.write(msgTx);
            waitingAckFrom = 0;
            waitingAckID = 0;
        break;
        
        case 140:// lanceur ON
            msgTx.id=0x402;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            msgTx.data[0]=1;
            msgTx.data[1]=0x02;
            can1.write(msgTx);
        break;
        
        case 141: // lanceur OFF
            msgTx.id=0x402;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            msgTx.data[0]=0;
            msgTx.data[1]=0x02;
            can1.write(msgTx);
        break;

        case 150:
            msgTx.id=0x404;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            msgTx.data[0]=1;
            msgTx.data[1]=BRAS_AVANT;
            
            can1.write(msgTx);
        break;
        
        case 151:
            msgTx.id=0x404;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
            
            msgTx.data[0]=0;
            msgTx.data[1]=BRAS_AVANT;
            
            can1.write(msgTx);
        break;
        
        case 10://Désactiver le stop
            isStopEnable = 0;
        break;
        case 11://Activer le stop
            isStopEnable = 1;
        break;
        case 20://Désactiver l'asservissement
            setAsservissementEtat(0);
        break;
        case 21://Activer l'asservissement
            setAsservissementEtat(1);
        break;
        
        case 22://Changer la vitesse du robot
            SendSpeed(speed,(unsigned short)angle);
            waitingAckFrom = 0;
            waitingAckID = 0;
            wait(0.2);
        break;
        
        case 30://Action tempo
            wait_ms(speed);
        break;
        
        /*case 40: // demande au telemetre la position d'un objet
            //SendRawId(TELEMETRE_RECHERCHE_OBJET);
            
            modeTelemetre = 1;
           
            //angle = angle /10;
            
            msgTx.id=TELEMETRE_OBJET;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=1;
            // indice du module sur le terrain
            msgTx.data[0] = (unsigned char)speed;
            
            
            // x sur 2 octets
            msgTx.data[0]=(unsigned char)speed;
            msgTx.data[1]=(unsigned char)(speed>>8);
            // y sur 2 octets
            msgTx.data[2]=(unsigned char)angle;
            msgTx.data[3]=(unsigned char)(angle>>8);
            // theta signé sur 2 octets
            //msgTx.data[4]=(unsigned char)theta;
            //msgTx.data[5]=(unsigned char)(theta>>8);
            msgTx.data[4]=0;
            msgTx.data[5]=0;
            
            can1.write(msgTx);
            
        break;*/
        
       /* case 130://Lancer mouvement de sortie de la zone de départ
            msgTx.id=ACTION_BIG_DEMARRAGE;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=1;
            msgTx.data[0] = (unsigned char)speed;
            can1.write(msgTx);
        break;*/
        
        default:
            return 0;//L'action n'existe pas, il faut utiliser le CAN
        
    }
    return 1;//L'action est spécifique.
    
}

/****************************************************************************************/
/* FUNCTION NAME: initRobot                                                             */
/* DESCRIPTION  : initialiser le robot                                                  */
/****************************************************************************************/
void initRobot(void) 
{
    //Enregistrement de tous les AX12 présent sur la carte
    /*AX12_register(5,  AX12_SERIAL2);
    AX12_register(18, AX12_SERIAL2);
    AX12_register(13, AX12_SERIAL2);
    AX12_register(1,  AX12_SERIAL1);
    AX12_register(11,  AX12_SERIAL1);
    AX12_register(8,  AX12_SERIAL1);
    AX12_register(7,  AX12_SERIAL2);*/
    
    //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
    //AX12_processChange();
    //runRobotTest();
    
}

/****************************************************************************************/
/* FUNCTION NAME: initRobotActionneur                                                   */
/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
/****************************************************************************************/
void initRobotActionneur(void)
{
    /*doAction(100,1,0);  
    doAction(100,2,0);
    doAction(110,0,0);
    doAction(120,0,0);
    doAction(131,0,0);*/
    
}

/****************************************************************************************/
/* FUNCTION NAME: runTest                                                               */
/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
/****************************************************************************************/
void runRobotTest(void) 
{
    /*
    int waitTime = 500;
    
    //Test des AX12 dans l'ordre
    doAction(111,0,0);//Fermeture pince arrière haute
    wait_ms(waitTime);
    doAction(110,0,0);//Ouverture pince arrière haute
    wait_ms(waitTime);
    doAction(113,0,0);//Fermeture pince arrière basse
    wait_ms(waitTime);
    doAction(112,0,0);//Ouverture pince arrière basse
    wait_ms(waitTime);
    doAction(115,0,0);//Fermeture porte arrière
    wait_ms(waitTime);
    doAction(114,0,0);//Ouverture porte arrière
    wait_ms(waitTime);
    doAction(101,0,0);//Fermer les portes avant
    wait_ms(waitTime);
    doAction(100,0,0);//Ouvrir les portes avant
    wait_ms(waitTime);
    doAction(103,0,0);//Descendre le peigne
    wait_ms(waitTime);
    doAction(102,0,0);//Remonter le peigne*/
}

/****************************************************************************************/
/* FUNCTION NAME: SelectStrategy                                                        */
/* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
/* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */
/****************************************************************************************/
int SelectStrategy(unsigned char id)
{
    switch(id)
    {
        // strat de match
        case 1:
            strcpy(cheminFileStart,"/local/strat1.txt");
            return FileExists(cheminFileStart);
        case 2:
            strcpy(cheminFileStart,"/local/strat2.txt");
            return FileExists(cheminFileStart);
        case 3:
            strcpy(cheminFileStart,"/local/strat3.txt");
            return FileExists(cheminFileStart);
        case 4:
            strcpy(cheminFileStart,"/local/strat4.txt");
            return FileExists(cheminFileStart);
        case 5:
            strcpy(cheminFileStart,"/local/strat5.txt");
            return FileExists(cheminFileStart);
        case 6:
            strcpy(cheminFileStart,"/local/strat6.txt");
            return FileExists(cheminFileStart);
        case 7:
            strcpy(cheminFileStart,"/local/strat7.txt");
            return FileExists(cheminFileStart);
        case 8:
            strcpy(cheminFileStart,"/local/strat8.txt");
            return FileExists(cheminFileStart);
        case 9:
            strcpy(cheminFileStart,"/local/strat9.txt");
            return FileExists(cheminFileStart);
        case 10:
            strcpy(cheminFileStart,"/local/strat10.txt");
            return FileExists(cheminFileStart);
        
        // strat de demo
        case 0x10:
            strcpy(cheminFileStart,"/local/moteur.txt");
            return FileExists(cheminFileStart);
        case 0x11:
#ifdef ROBOT_BIG
            strcpy(cheminFileStart,"/local/bras.txt");
#else
            strcpy(cheminFileStart,"/local/porteAvant.txt");
#endif
            return FileExists(cheminFileStart);
        case 0x12:
#ifdef ROBOT_BIG
            strcpy(cheminFileStart,"/local/balancier.txt");
#else
            strcpy(cheminFileStart,"/local/mainTourneuse.txt");
#endif
            return FileExists(cheminFileStart);
        default:
            strcpy(cheminFileStart,"/local/strat1.txt");
            return 0;
    }
}

/****************************************************************************************/
/* FUNCTION NAME: needToStop                                                            */
/* DESCRIPTION  : Savoir si il faut autoriser le stop du robot via balise               */
/****************************************************************************************/
unsigned char needToStop(void)
{
    return isStopEnable;
}

/****************************************************************************************/
/* FUNCTION NAME: doBeforeEndAction                                                     */
/* DESCRIPTION  : Terminer les actions du robot 1s avant la fin du match                */
/****************************************************************************************/
void doBeforeEndAction(void)
{
    doAction(110,0,0);//Ouverture pince arrière haute
    doAction(112,0,0);//Ouverture pince arrière basse
    doAction(114,0,0);//Ouverture porte arrière
    doAction(100,0,0);//Ouvrir les portes avant
    doAction(102,0,0);//Remonter le peigne
}

#endif