File content as of revision 108:890094ee202a:

#include "mbed.h"

#define PLAN_B
#define OUT_USB

#include "defines.h"

#include "QEI.h"
#include "Map.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);
    
    Odometry 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> instanceAsserv(&odometry);
#else
    aserv_planB asserv(odometry,motorL,motorR);
#endif

//           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
    
    logger.printf("[done]\r\n");
    
    //           Fin Init.          //
    // *--------------------------* //
    
    // *--------------------------* //
    //              MIP             //
    
    logger.printf("Appuyer sur le bouton pour mettre ne position\r\n");
    
    while(!bp); // On attend le top de mise en position
    
    logger.printf("MIP........................");
    
    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");
    
    //                              //
    // *--------------------------* //

    bool continuer = true;
    
    #ifdef PLAN_A
        instanceAsserv.setGoal(300.0f,200.0f,0.0f);
        logger.printf("GOAL SET... RUNNING!\r\n");
        
        char state = 0;
        
        while(continuer)
        {
            #define test
            #ifdef test            
                if(state == 0 && instanceAsserv.isArrivedRho())
                {
                    state = 1;
                    logger.printf("Arrive en 0,0 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    instanceAsserv.setGoal(300.0f,200.0f,0.0f);
                }
                else if(state == 1 && instanceAsserv.isArrivedRho())
                {
                    state = 2;
                    logger.printf("Arrive en 200,200 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    instanceAsserv.setGoal(0.0f,300.0f,0.0f);
                }
                else if(state == 2 && instanceAsserv.isArrivedRho())
                {
                    state = 0;
                    logger.printf("Arrive en 0,200 !!!!!\r\n");
                    motorR.setSpeed(0.0f);
                    motorL.setSpeed(0.0f);
                    wait(5);
                    instanceAsserv.setGoal(0.0f,0.0f,0.0f);
                }
            #endif
        }
    #else
        asserv.setGoal(1,300,0);
        logger.printf("Attente\r\n");
        while(!asserv.isArrived())wait(0.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
    
    while(1);
}

void update()
{
    float dt = t.read();
    t.reset();
    
    odometry.update(dt);
    
    #ifdef PLAN_A
        instanceAsserv.update(dt);
        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));
    #else
        asserv.update(dt);
    #endif
}