File content as of revision 48:03da1aead032:

#include "func.h"

void pressed(void)
{
    if(SIMON_i==0) {
        roboclaw.ForwardM1(0);
        roboclaw.ForwardM2(0);
        SIMON_i++;
    }
}

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

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

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

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

void JPO(void)
{
    char c = logger.getc();
    if(c=='z') {
        if (SIMON_state != 1) {
            SIMON_state = 1;
            logger.printf("Avant (Z) \r\n");
            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
        }
    } else if(c == 's') {
        if (SIMON_state != 2) {
            SIMON_state = 2;
            logger.printf("Stop (S) \r\n");
            roboclaw.SpeedAccelM1(accel_angle, 0);
            roboclaw.SpeedAccelM2(accel_angle, 0);
        }
    } else if(c == 'd') {
        if (SIMON_state != 3) {
            SIMON_state = 3;
            logger.printf("Droite (D) \r\n");
            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
            roboclaw.SpeedAccelM2(accel_angle, vitesse_angle);
        }
    } else if(c == 'q') {
        if (SIMON_state != 4) {
            SIMON_state = 4;
            logger.printf("Gauche (Q)\r\n");
            roboclaw.SpeedAccelM1(accel_angle, vitesse_angle);
            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
        }
    } else if(c == 'x') {
        if (SIMON_state != 5) {
            SIMON_state = 5;
            roboclaw.SpeedAccelM1(accel_angle, -vitesse_angle);
            roboclaw.SpeedAccelM2(accel_angle, -vitesse_angle);
        }
    } else {
        if (SIMON_state != 0) {
            roboclaw.SpeedAccelM1M2(accel_angle, 0, accel_angle, 0);
            SIMON_state = 0;
        }
    }
}

void goHome(void)
{
    //while(EZ==false) ZMot.step(1, 0, DELAY);
    while(SIMON_ER==false) RMot.step(1, 0, DELAY);
    RMot.step(10, 1, DELAY+0.01);
    RMot.step(5, 0, DELAY);
    while(SIMON_EL==false) LMot.step(1, 0, DELAY);
    LMot.step(10, 1, DELAY+0.01);
    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)
{
    CAMP.fall(&changeCamp);
    START.rise(&go);
    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 changeCamp(void)
{
    if(SCouleur==VERT) {
        SCouleur = VIOLET;
        LEDR = 1;
        LEDV = 0;
    }
    else {
        SCouleur = VERT;
        LEDV = 1;
        LEDR = 0;        
    }
}

void go(void)
{
    SStart = 1;
}