File content as of revision 45:b53ae54062c6:

#include "func.h"

void pressed(void)
{
    if(i==0) {
        roboclaw.ForwardM1(ADR, 0);
        roboclaw.ForwardM2(ADR, 0);
        i++;
    }
}

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

void ELpressed(void)
{
    //bleu = 1;
    EL = true;
}
void ELunpressed(void)
{
    //bleu = 0;
    EL = false;
}

void EZpressed(void)
{
    //blanc = 1;
    EZ = true;
}
void EZunpressed(void)
{
    //blanc = 0;
    EZ = false;
}

void ERpressed(void)
{
    //rouge = 1;
    ER = true;
}
void ERunpressed(void)
{
    //rouge = 0;
    ER = 0;
}

void JPO(void)
{
    char c = logger.getc();
    if(c=='z') {
        if (state != 1) {
            state = 1;
            logger.printf("Avant (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;
            logger.printf("Stop (S) \r\n");
            roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
            roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
        }
    } else if(c == 'd') {
        if (state != 3) {
            state = 3;
            logger.printf("Droite (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("Gauche (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.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
            roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
        }
    } else {
        if (state != 0) {
            roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
            state = 0;
        }
    }
}

void goHome(void)
{
    while(EZ==false) ZMot.step(1, 0, DELAY);
    while(ER==false) RMot.step(1, 0, DELAY);
    RMot.step(10, 1, DELAY);
    RMot.step(5, 0, DELAY);
    while(EL==false) LMot.step(1, 0, DELAY);
    LMot.step(10, 1, DELAY);
    LMot.step(5, 0, DELAY);
}

void checkAround(void)
{
    if(capt1 > SEUIL+0.1) bleu = 1;
    else bleu = 0;
    if(capt2 > SEUIL) blanc = 1;
    else blanc = 0;
    if(capt3 > SEUIL+0.1) rouge = 1;
    else rouge = 0;
}

void init_ax12(void)
{
    left_hand.setMode(0);
    wait_ms(10);
    right_hand.setMode(0);
    wait_ms(50);
    left_hand.setMode(0);
    wait_ms(10);
    right_hand.setMode(0);
    wait_ms(50);
    right_hand.setGoal(0);
    left_hand.setGoal(0);
    wait(2);
    right_hand.setGoal(180);
    left_hand.setGoal(180);
    wait(2);
}

void init_interrupt(void)
{
    mybutton.fall(&pressed);
    mybutton.rise(&unpressed);

    EndL.fall(&ELpressed);
    EndL.rise(&ELunpressed);
    EndZ.fall(&EZpressed);
    EndZ.rise(&EZunpressed);
    EndR.fall(&ERpressed);
    EndR.rise(&ERunpressed);
    ticker.attach_us(&update_main,dt); // 100 Hz
}

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