File content as of revision 100:7c7174048302:

#include "entete.h"

#include "AX12/AX12.h"
#include "ControlleurPince/ControlleurPince.h"

DigitalIn CAMP(PA_15);
DigitalIn START(PB_7);
DigitalOut LEDR(PC_2);
DigitalOut LEDV(PC_3);
DigitalIn button(USER_BUTTON);
int SCouleur = VERT;

BusOut drapeau (PC_8, PC_6, PC_5);
RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
DigitalIn start(PB_7);

AX12 Parasol(PA_9, PA_10, 2, 250000);

/* Sharps */
AnalogIn Rcapt1(PA_4);
int RvalRcapt1 = 0;
AnalogIn Rcapt2(PB_0);
int RvalRcapt2 = 0;
AnalogIn Rcapt3(PC_1);
int RvalRcapt3 = 0;
int Ravance = 1;
float R_SEUIL_SHARP = 1;

/* Pour la pince */
AX12 left_hand(PA_9, PA_10, 3, 250000);
AX12 right_hand(PA_9, PA_10, 1, 250000);
Stepper RMot(NC, PA_8, PC_7, PB_15, 4);
Stepper ZMot(NC, PB_4, PB_10, PB_14, 5);
Stepper LMot(NC, PB_5, PB_3, PB_13, 4);
DigitalIn EnR(PB_15);
DigitalIn EnZ(PB_14);
DigitalIn EnL(PB_13);

ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL,left_hand,right_hand);
int SIMON_state = 's';
Serial logger(PA_2, PA_3);

void JPO(void);

/* Debut du programme */
int main(void)
{
   
    init_globals();
    
   /* if (SCouleur == VERT) {
        // Chateau
        end.attach(&endFonc, 90);
        GotoDist(9.0,2000,2000,2000);
        
        // Callage
        GotoArr(5);
        R_SEUIL_SHARP = 0.25;
        
        // Cabane 1
        GotoDist(1.25);
        GotoThet(-PI/2.f);
        R_SEUIL_SHARP = 0.35;
        GotoDist(2.75);
        R_SEUIL_SHARP = 1;
        controlleurPince.setPos(-1,150);
        GotoDist(2.25);
        
        // Cabane 2
        GotoArr(2);
        R_SEUIL_SHARP = 0.35;
        GotoThet(PI/2.f);
        GotoDist(1.75);
        GotoThet(PI/2.f);
        GotoArr(3);
        
        // Coquillage 1
        R_SEUIL_SHARP = 0.35;
        GotoDist(3.5);
        controlleurPince.setPos(0);
        controlleurPince.angle(50);
        
        GotoThet(-0.3f);
        
        GotoDist(2);
        
        controlleurPince.close();
        controlleurPince.setPos(-1,70);
        
        // Coquillage 2
        GotoThet(PI/2.f-0.3f);
        R_SEUIL_SHARP = 1;
        controlleurPince.angle(50);
        GotoDist(2);
        controlleurPince.close();
        
        // Fin !!!
        GotoThet(PI/2.f);
        GotoDist(1.75);
        controlleurPince.angle(50);
        GotoArr(1);*/

    R_SEUIL_SHARP = 0.35;
    drapeau = 3;
    while (1)
       JPO();   

    while(1);
}

void Sharps() {
    if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
    else RvalRcapt1--;
    RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
    RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
    
    if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
    else RvalRcapt2--;
    RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
    RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
    
    if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
    else RvalRcapt3--;
    RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
    RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
    
    if ((RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5))
        Ravance = 0;
    else 
        Ravance = 1;
        
    if (Ravance == 0)
        drapeau = 1;
    else 
        drapeau = 2;
}

void endFonc () {
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    wait(1);
    Parasol.setGoal(300);
    wait(1);
    Parasol.setMaxTorque(1000);
    while(1);
}

void init_globals() {
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);


    controlleurPince.init();
    Parasol.setMode(0);
    Parasol.setMaxTorque(200);
    Parasol.setGoal(150);
    wait(0.5);
    Parasol.setGoal(160);
    wait(0.5);
    Parasol.setGoal(150);
    wait(1);
    Parasol.setMaxTorque(0);
    
    controlleurPince.home();
    controlleurPince.setPos(130,0,0);
    controlleurPince.open();
    
    while(START == 1)
    {
        LEDR = 1;
        LEDV = 1;
        wait(0.5);
        LEDR = 0;
        LEDV = 0;
        wait(0.5);
    }
   
    wait(1);
    LEDV = 1;
    depart();
}


void JPO(void)
{
    char c = logger.getc();
    printf("Coucou :)\n\r");
    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 (c == 'c') {
        if (SIMON_state != 6) {
            SIMON_state = 6;
            
        }
    } else if (c == 'o') {
        if (SIMON_state != 7) {
            SIMON_state = 7;
        }
    } else if (c == 'h') {
        if (SIMON_state != 8) {
            SIMON_state = 8;
        }
    } else if (c == 'b') {
        if (SIMON_state != 9) {
            SIMON_state = 9;
        }
    }
    else {
        if (SIMON_state != 0) {
            roboclaw.SpeedAccelM1M2(accel_angle, 0, 0);
            SIMON_state = 0;
        }
    }
}