File content as of revision 52:1056dd73a748:

#include "func.h"

void init_interrupt(void)
{
    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 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, 0);
            SIMON_state = 0;
        }
    }
}

void goHome(void)
{
    goHomeZ();
    goHomeL();
    goHomeR();
}

void goHomeL(void)
{
    while(EnL==true) LMot.step(1, BAS, DELAY);
    LMot.step(10, HAUT, DELAY+0.01);
    LMot.step(5, BAS, DELAY);
}

void goHomeZ(void)
{
    while(EnZ==true) ZMot.step(1, BAS, DELAY);
    ZMot.step(10, HAUT, DELAY+0.01);
    ZMot.step(5, BAS, DELAY);
}

void goHomeR(void)
{
    while(EnR==true) RMot.step(1, BAS, DELAY);
    RMot.step(10, HAUT, DELAY+0.01);
    RMot.step(5, BAS, DELAY);
}

void checkAround(void)
{
    if(capt1.read() > SEUIL+0.1) SGauche = true;
    else SGauche = false;
    if(capt2.read() > SEUIL) SDevant = true;
    else SDevant = false;
    if(capt3.read() > SEUIL+0.1) SDroite = true;
    else SDroite = false;
    logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
}

void homologation(void)
{
    goHomeZ();
    ZMot.mm(50, HAUT);
    goHomeL();
    goHomeR();
    LMot.mm(50, HAUT);
    RMot.mm(50, HAUT);
    while(START==0) logger.printf("%1.3f\t%1.3f\t%1.3f\n\r", capt1.read(), capt2.read(), capt3.read());
    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
    while(1) {
        while(capt2.read() < SEUIL) checkAround();
        while(1) roboclaw.SpeedAccelM1M2(accel_dista, 0, 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 changeCamp(void)
{
    if(SCouleur==VERT) {
        SCouleur = VIOLET;
        LEDR = 1;
        LEDV = 0;
    } else {
        SCouleur = VERT;
        LEDV = 1;
        LEDR = 0;
    }
}

void depart(void)
{
    while(button==1) {
        if(CAMP==0) {
            changeCamp();
        }
        wait_ms(100);
    }

    if(SCouleur == VERT)logger.printf("Couleur VERT !\n\r");
    else logger.printf("Couleur ROUGE !\n\r");

    while(START==0) {
        drapeau = SSchema;
        if(CAMP==0) {
            SSchema++;
            if(SSchema == 6) SSchema = 1;
            drapeau = SSchema;
        }
        wait_ms(100);
    }
    logger.printf("Schema numero : %d !\n\r", SSchema);
}