File content as of revision 123:55e5e9acc541:

#include "mbed.h"

#include "defines.h"

#include "QEI.h"
#include "Map.h"

#include "Objectif.h"
#include "Obj_clap.h"
#include "Obj_pince.h"
#include "Obj_depot.h"


#include "AX12.h"

#ifdef PLAN_A
    #include "Odometry.h"
    #include "Asserv.h"
#else
    #include "Odometry2.h"
    #include "planB.h"
#endif

#include "Motor.h"

void update();

// *--------------------------* //
//         Déclarations         //

//        Decl. logger          //

Serial logger(OUT_TX, OUT_RX); // tx, rx

//        Decl. timer           //

Timer t;
Ticker ticker;

//         Decl. AX12           //

AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);

//        Decl. Moteurs         //

PwmOut pw1(PWM_MOT1);
DigitalOut dir1(DIR_MOT1);
PwmOut pw2(PWM_MOT2);
DigitalOut dir2(DIR_MOT2);

Motor motorL(PWM_MOT1,DIR_MOT1);
Motor motorR(PWM_MOT2,DIR_MOT2);

//         Decl. Sharps         //

AnalogIn sharp_devant(SHARP_D);
AnalogIn sharp_devant_droite(SHARP_DD);
AnalogIn sharp_devant_gauche(SHARP_DG);
AnalogIn sharp_arriere_gauche(SHARP_AG);
AnalogIn sharp_arriere_droite(SHARP_AD);

//        Decl. Boutons         //

DigitalIn bp(BP_DESSUS);
DigitalIn tirette(TIRETTE_DESSUS);
DigitalIn couleur(COULEUR_DESSUS);

//       Decl. Odometrie        //

#ifdef PLAN_A
    QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
    QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
    
    Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
#else
    QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
    QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
    
    Odometry2 odometry(&qei_left,&qei_right,63.84/2.,63.65/2.,252);
#endif

//         Decl. Asserv         //

#ifdef PLAN_A
    Asserv<float> asserv(&odometry);
#else
    aserv_planB asserv(odometry,motorL,motorR);
#endif

//           Decl. IA           //

Map terrain;
std::vector<Objectif*> objectifs;
char couleurRobot = COULEUR_JAUNE;

//           Fin Decl.          //
// *--------------------------* //


int main()
{
    #ifdef OUT_USB
        logger.baud((int)921600);
    #endif
    
    // *--------------------------* //
    //             Init             //
    
    logger.printf("Initialisation.............");
    
    //         Init. AX12           //
    
    ax12_pince.setMode(0);
    ax12_brasG.setMode(0);
    ax12_brasD.setMode(0);
    
    //        Init. Moteurs         //
    
    motorL.setSpeed(0);
    motorR.setSpeed(0);
    
    //        Init. Sharps          //
    
    //        Init. Boutons         //
    
    //       Init. Odometrie        //
    
    #ifdef PLAN_A
        odometry.setTheta(0);
        odometry.setX(0);
        odometry.setY(0);
    #else
        odometry.setTheta(PI/2);
        odometry.setX(0);
        odometry.setY(0);
    #endif
    
    //           Init. IA           //
    
    terrain.build();
    
    logger.printf("[done]\r\n");
    
    //           Fin Init.          //
    // *--------------------------* //
    
    // *--------------------------* //
    //              MIP             //
    
    logger.printf("Appuyer sur le bouton pour mettre en position\r\n");
    
    while(!bp); // On attend le top de mise en position
    
    logger.printf("MIP........................");
    
    odometry.update(1);
    
    /*odometry.setTheta(PI/2);
    odometry.setX(1000);
    odometry.setY(400);*/
    
    ax12_pince.setMaxTorque(MAX_TORQUE);
    ax12_brasG.setMaxTorque(MAX_TORQUE);
    ax12_brasD.setMaxTorque(MAX_TORQUE);
    
    ax12_pince.setGoal(PINCE_FERMEE);
    ax12_brasG.setGoal(BRASG_OUVERT);
    ax12_brasD.setGoal(BRASD_OUVERT);
    wait(0.5);
    ax12_pince.setGoal(PINCE_OUVERTE);
    ax12_brasG.setGoal(BRASG_FERME);
    ax12_brasD.setGoal(BRASD_FERME);
    wait(1.5);
    ax12_pince.setMaxTorque(0);
    ax12_brasG.setMaxTorque(0);
    ax12_brasD.setMaxTorque(0);
    
    logger.printf("[done]\r\n");
    
    //                              //
    // *--------------------------* //
    
    // *--------------------------* //
    //            Asserv            //
    
    logger.printf("Demarrage asserv...........");
    t.start();
    ticker.attach_us(&update,10000); //100Hz
    logger.printf("[done]\r\n");
    
    //                              //
    // *--------------------------* //
    
    // *--------------------------* //
    //     Tirrette + couleur       //
    
    while(tirette); // La tirrette
    
    if(couleur == COULEUR_JAUNE)
    {
        couleurRobot = COULEUR_JAUNE;
        
        odometry.setTheta(PI/2);
        odometry.setX(1000);
        odometry.setY(177);
        
        objectifs.push_back( new Obj_depot(1000, 350, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_DEPOT_PC);
        
        objectifs.push_back( new Obj_depot(1000, 350+100, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_DEPOT_P);
        objectifs.push_back( new Obj_depot(1000, 350+2*100, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_DEPOT_P);
        objectifs.push_back( new Obj_depot(1000, 350+3*100, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_DEPOT_P);
        objectifs.push_back( new Obj_depot(1100, 330+3*100, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_DEPOT_P);
        objectifs.push_back( new Obj_depot(900, 330+3*100, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_DEPOT_P);
        
        //objectifs.push_back( new Obj_pince(800, 620, 800, 700, PI/2, &ax12_pince) );
        //objectifs.back()->setId(IDO_PC1);
        objectifs.push_back( new Obj_pince(1750, 250+350, 1750, 250+190, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_PC2);
        objectifs.push_back( new Obj_pince(1750, 90+350, 1750, 90+190, -PI/2, &ax12_pince) );
        objectifs.back()->setId(IDO_P5);
        objectifs.push_back( new Obj_pince(1850-247, 90+247, 1850-130, 90+130, -PI/4, &ax12_pince) );
        objectifs.back()->setId(IDO_P6);
        
        
        objectifs.push_back( new Obj_clap(1750, 192, -PI/4, 1750, 400, PI/2, &ax12_brasG, &ax12_brasD) );
        
        objectifs.push_back( new Obj_pince(1770-247, 1100-247, 1770-135, 1100-135, PI/4, &ax12_pince) );
        objectifs.back()->setId(IDO_P7);
        objectifs.push_back( new Obj_pince(1355-247, 870-247, 1355-135, 870-135, PI/4, &ax12_pince) );
        objectifs.back()->setId(IDO_P4);
    }
    else
    {
        couleurRobot = COULEUR_VERTE;
    }
    
    
    //                              //
    // *--------------------------* //

    bool continuer = true;
    
    // *--------------------------* //
    //              IA              //
    
    #define IA
    #ifdef IA
    
    while(continuer)
    {
        if(terrain.getHeight(odometry.getX(), odometry.getY()) >= 32000)
        {
            logger.printf("Collision, echappement.....");
            
            // Réduction de la taille des obstacles
            for(unsigned int i=0;i < terrain.obstacles.size();i++)
                while(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
                    terrain.obstacles[i]->setRobotRadius(terrain.obstacles[i]->getRobotRadius()-1);
            
            logger.printf("[done]\r\n");
        }
        
        logger.printf("Recherche d'objectif.......");
        int objAct = 0;
        bool newObj = false;
        for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
        {
            if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
                continue;
            if(!objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
                continue;
            int retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 10);
            if(retourAStar == 1) // L'objectif est atteignable !!
            {
                newObj = true;
                break;
            }
            
            if(retourAStar == 3) // Overload memoire
                retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 25);
            
            if(retourAStar == 1) // L'objectif est atteignable !!
            {
                newObj = true;
                break;
            }
            
            if(retourAStar == 3) // Overload memoire
                retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 50);
            
            if(retourAStar == 1) // L'objectif est atteignable !!
            {
                newObj = true;
                break;
            }
        }
        
        logger.printf("[done]\r\n");
        
        // On remet la taille des obstacles
        for(unsigned int i=0;i < terrain.obstacles.size();i++)
            terrain.obstacles[i]->setRobotRadius(ROBOTRADIUS);
        
        if(newObj)
        {
            logger.printf("-> Objectif : %d\r\n",objectifs[objAct]->getId());
            
            // Déplacement vers le nouvel objectif //
            
            for(int i=1;i<terrain.path.size()-1;i++)
            {
                logger.printf("Prochain point : %d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
                asserv.setGoal(terrain.path[i].x,terrain.path[i].y);
                while(!asserv.isArrived())wait(0.1);
            }
            logger.printf("Dernier point : %.3f\t%.3f\r\n",objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta()*180/PI);
            asserv.setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
            while(!asserv.isArrived())wait(0.1);
            
            
            // Execution de l'objectif //
            
            logger.printf("Execution de l'objectif....");
            
            objectifs[objAct]->run();
            logger.printf("[done]\r\n");
        }
        else
        {
            logger.printf("-> Nothind to be done ... :D \r\n");
            
            ax12_brasG.setMaxTorque(MAX_TORQUE);
            ax12_brasD.setMaxTorque(MAX_TORQUE);
            
            ax12_brasG.setGoal(130);
            ax12_brasD.setGoal(BRASD_FERME);
            wait(1);
            ax12_brasG.setGoal(BRASG_FERME);
            ax12_brasD.setGoal(215);
            wait(1);
            ax12_brasD.setGoal(BRASD_FERME);
            wait(1);
            
            ax12_brasG.setMaxTorque(0);
            ax12_brasD.setMaxTorque(0);
        }
    }
    
    #endif
    
    #ifdef PLAN_A
        asserv.setGoal(300.0f,200.0f,0.0f);
        logger.printf("GOAL SET... RUNNING!\r\n");
        
        char state = 0;
        
        while(continuer)
        {
            //#define test
            #ifndef test
                if(state == 0 && asserv.isArrived())
                {
                    state = 1;
                    logger.printf("Arrive en 0,0 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    asserv.setGoal(0.0f,0.0f,0.0f);
                }
                else if(state == 1 && asserv.isArrived())
                {
                    state = 0;
                    logger.printf("Arrive en 0,200 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    asserv.setGoal(0.0f,0.0f,0.0f);
                }
            #endif
            #ifdef test            
                if(state == 0 && asserv.isArrivedRho())
                {
                    state = 1;
                    logger.printf("Arrive en 0,0 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    asserv.setGoal(300.0f,200.0f,0.0f);
                }
                else if(state == 1 && asserv.isArrivedRho())
                {
                    state = 2;
                    logger.printf("Arrive en 200,200 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    asserv.setGoal(0.0f,300.0f,0.0f);
                }
                else if(state == 2 && asserv.isArrivedRho())
                {
                    state = 0;
                    logger.printf("Arrive en 0,200 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    asserv.setGoal(0.0f,0.0f,0.0f);
                }
            #endif
        }
    #else
        asserv.setGoal(1,5);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        asserv.setGoal(1,10);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        asserv.setGoal(1,15);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        asserv.setGoal(1,20);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        asserv.setGoal(1,25);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        asserv.setGoal(1,30);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        asserv.setGoal(0,0,PI/2);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.1);
        logger.printf("Arrive\r\n");
        while(1);
        /*logger.printf("Arrivé\r\n");
        wait(1);
        asserv.setGoal(-300,300,0);
        while(!asserv.isArrived())wait(0.1);
        wait(1);
        asserv.setGoal(-300,-1,0);
        while(!asserv.isArrived())wait(0.1);
        wait(1);
        asserv.setGoal(-300,-300,0);
        while(!asserv.isArrived())wait(0.1);
        wait(1);
        asserv.setGoal(1,-300,0);
        while(!asserv.isArrived())wait(0.1);
        wait(1);
        asserv.setGoal(300,-300,0);
        while(!asserv.isArrived())wait(0.1);
        wait(1);
        asserv.setGoal(0,0,0);*/
    #endif
}

void update()
{
    float dt = t.read();
    t.reset();
    
    odometry.update(dt);
    asserv.update(dt);
    
    #ifdef PLAN_A
        float phi_r = (float)instanceAsserv.getPhiR();
        float phi_l = (float)instanceAsserv.getPhiL();
        float phi_max = (float)instanceAsserv.getPhiMax();
        
        motorR.setSpeed(0.08+(phi_r/phi_max));
        motorL.setSpeed(0.08+(phi_l/phi_max));
    #endif
}