File content as of revision 39:ca4dd3faffa8:

#include "Odometry/Odometry.h"
#include "map/map.h"

#define dt 10000
#define ATTENTE 0
#define GO 1
#define STOP 2
#define ENTRAXE 248.25

InterruptIn mybutton(USER_BUTTON);
DigitalIn button(USER_BUTTON);
DigitalOut led(LED1);
Ticker ticker;
//Serial pc(USBTX, USBRX);
Serial logger (PA_9, PA_10);
RoboClaw roboclaw(460800, PA_11, PA_12);
Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);

int i = 0;

void update_main(void);
void init(void);
void pressed(void);
void unpressed(void);

/** Debut du programme */
int main(void)
{
    init();
    double angle_v = 2*PI/5;
    double distance_v = 200.0;
    float the;
    
    odo.setPos(0, 1000, 0);
    point dep (odo.getX(), odo.getY());
    point arr (1400, 1000);
    
    map m;
    m.addObs(obsCarr (550, 1030, 250, 230));
    m.addObs(obsCarr (740, 720, 240, 240));
    m.addObs(obsCarr (1170, 1260, 220, 220));
    
    /*m.addObst (carre (1000, 1000, 210, 210));
    m.addObst (carre (1300, 700, 180, 180));*/
    m.FindWay (dep, arr);
    logger.printf("On a cherche un chemin\n\r");
    
    if (m.getEnded()) {
        nVector<pointParcours> p = m.getParc ();
        logger.printf ("Il y a %d points de parcours\n\r", p.size());
        for (int i = 0; i < p.size (); i++) {
            logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
            //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
            odo.GotoXY(p[i].getX(), p[i].getY());
        }
    }
    else {
        logger.printf("Chemin pas trouve ...");
    }
    
    //odo.GotoThet(PI);
    odo.GotoThet(0);
    //odo.TestEntraxe(3);
    
    //odo.GotoThet(-PI/2);
    wait(2000);
    while (1);
    //odo.GotoXYT(2250,500,0);*/
    /*roboclaw.ForwardM1(ADR, 0);
    roboclaw.ForwardM2(ADR, 0);
    int state = 0;
    while(1)
    {
       // while(logger.readable()) 
        //{
            char c = logger.getc();
            if(c=='z')
            {
                if (state != 1) {
                    state = 1;
                    logger.printf("Z\r\n");
                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
                }
            }
            else if(c == 's')
            {
                if (state != 2) {
                    state = 2;
                    roboclaw.ForwardM1(ADR, 0);
                    roboclaw.ForwardM2(ADR, 0);
                }
            }
            else if(c == 'd')
            {
                if (state != 3) {
                    state = 3;
                    logger.printf("D\r\n");
                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
                    roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
                }
            }
            else if(c == 'q')
            {
                if (state != 4) {
                    state = 4;
                    logger.printf("Q\r\n");
                    roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
                }
            }
            else if(c == 'x')
            {
                if (state != 5) {
                    state = 5;
                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
                }
            }
            else if (c == '\0') { ; }
            else {
                if (state != 0) {
                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
                    state = 0;
                }
            }
       // }
       // roboclaw.ForwardM1(ADR, 0);
       // roboclaw.ForwardM2(ADR, 0);
    }*/
}

void init(void)
{
    roboclaw.ForwardM1(ADR, 0);
    roboclaw.ForwardM2(ADR, 0);
    logger.baud(9600);
    logger.printf("Hello from main !\n\r");
    wait_ms(500);
    
    odo.setPos(0, 1000, 0);
        
    while(button);
    wait(1);
    mybutton.fall(&pressed);
    mybutton.rise(&unpressed);
    ticker.attach_us(&update_main,dt); // 100 Hz
    
    logger.printf("End init\n\r");
}

void update_main(void)
{
    odo.update_odo();
    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
    //if(pc.readable()) if(pc.getc()=='l') led = !led;
}

void pressed(void)
{
    if(i==0) {
        roboclaw.ForwardM1(ADR, 0);
        roboclaw.ForwardM2(ADR, 0);
        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
        i++;
    }
}

void unpressed(void)
{
    if(i==1) {
        i--;
    }
}