File content as of revision 43:d5aaff7d2bec:

#include "func.h"

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

/* Déclaration des différents éléments de l'IHM */
InterruptIn mybutton(USER_BUTTON);
DigitalIn button(USER_BUTTON);
DigitalOut led(LED1);
DigitalOut bleu(PC_5);
DigitalOut blanc(PC_6);
DigitalOut rouge(PC_8);

/* AX12 */
AX12 left_hand(PA_15, PB_7, 4, 250000);
AX12 right_hand(PA_15, PB_7, 1, 250000);

/* Sharp */
AnalogIn capt1(PA_4);
AnalogIn capt2(PB_0);
AnalogIn capt3(PC_1);
AnalogIn capt4(PC_0);

/* Moteurs pas à pas */
Stepper RMot(NC, PB_10, PA_8);
Stepper ZMot(NC, PB_5, PB_4);
Stepper LMot(NC, PB_3, PA_10);
/* Fins de course */
InterruptIn EndR(PB_15);
InterruptIn EndZ(PB_14);
InterruptIn EndL(PB_13);

Ticker ticker;

Serial logger(USBTX, USBRX);
//Serial logger(PA_9, PA_10);
RoboClaw roboclaw(460800, PA_11, PA_12);
Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);

int i = 0, state = 0;
bool EL = false, EZ = false, ER = false;

void update_main(void);
void init(void);

/* Debut du programme */
int main(void)
{
    init();
    /* Code AStar */

    /*double angle_v = 2*PI/5;
    double distance_v = 200.0;
    std::vector<SimplePoint> path;
    Map map;


    //Construction des obstacles
    map.build();

    float x=odo.getX();
    float y=odo.getY();
    float the = 0;

    map.AStar(x, y, 1600, 1000, 75);
    path = map.path;

    for(int i=0; i<path.size();i++) {
        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
        odo.GotoXYT(path[i].x, path[i].y, the);
    }

    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
    path = map.path;

    for(int i=0; i<path.size();i++) {
        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
        odo.GotoXYT(path[i].x, path[i].y, the);
    }*/

    //odo.GotoThet(PI);
    //odo.GotoThet(0);
    //odo.TestEntraxe(3);

    //odo.GotoThet(-PI/2);
    //wait(2000);
    //odo.GotoXYT(2250,500,0);

    //odo.TestEntraxe(5);
    //odo.Forward(1000);
    while(1)
    {
        JPO();
    }
}

void init(void)
{
    roboclaw.ForwardM1(ADR, 0);
    roboclaw.ForwardM2(ADR, 0);
    logger.baud(9600);
    logger.printf("Hello from main !\n\r");
    wait_ms(500);
    bleu = 1;
    blanc = 1;
    rouge = 1;
    wait_ms(1000);
    bleu = 0;
    blanc = 0;
    rouge = 0;
    
    //odo.setPos(0, 1000, 0);
    while(button);
    wait(1);

    mybutton.fall(&pressed);
    mybutton.rise(&unpressed);

    EndL.fall(&ELpressed);
    EndL.rise(&ELunpressed);
    EndZ.fall(&EZpressed);
    EndZ.rise(&EZunpressed);
    EndR.fall(&ERpressed);
    EndR.rise(&ERunpressed);

    wait_ms(100);
    ticker.attach_us(&update_main,dt); // 100 Hz
    logger.printf("End init\n\r");
}

void update_main(void)
{
    odo.update_odo();
    checkAround();
}