File content as of revision 71:5590dbe8393a:

#include "func.h"
#include "map.h"

#include "ControlleurPince.h"

/* Déclaration des différents éléments de l'IHM */

DigitalIn CAMP(PA_15);
DigitalIn START(PB_7);
DigitalOut LEDR(PC_2);
DigitalOut LEDV(PC_3);

BusOut drapeau(PC_8, PC_6, PC_5);

InterruptIn mybutton(USER_BUTTON);
DigitalIn button(USER_BUTTON);

DigitalIn ChannelA1(PA_1);
DigitalIn ChannelB1(PA_0);
DigitalIn ChannelA2(PA_5);
DigitalIn ChannelB2(PA_6);

DigitalOut bleu(PC_5);
DigitalOut blanc(PC_6);
DigitalOut rouge(PC_8);

AX12 left_hand(PA_9, PA_10, 3, 250000);
AX12 right_hand(PA_9, PA_10, 2, 250000);

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

/* Moteurs pas à pas */
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);
/* Fins de course */
InterruptIn EndR(PB_15);
InterruptIn EndZ(PB_14);
InterruptIn EndL(PB_13);
DigitalIn EnR(PB_15);
DigitalIn EnZ(PB_14);
DigitalIn EnL(PB_13);

ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL, left_hand, right_hand);

Ticker ticker;
//Serial logger(USBTX, USBRX);
Serial logger(PA_2, PA_3);
RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);

int SIMON_i = 0, SIMON_state = 0, SCouleur = VERT, SStart = 0, SSchema = 1;
bool SIMON_EL = false, SIMON_EZ = false, SIMON_ER = false, SGauche = false, SDevant = false, SDroite = false;

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

/* Debut du programme */
int main(void)
{
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    
    //logger.printf("Depart homologation !\n\r");
    //homologation();
    
    controlleurPince.init();
    controlleurPince.home();
    wait(1);
    controlleurPince.open();
    logger.printf("z 10 \r\n");
    controlleurPince.setPos(10,-1);
    wait(1);
    logger.printf("z 20 \r\n");
    controlleurPince.setPos(20,-1);
    wait(1);
    controlleurPince.close();
    logger.printf("d 30 \r\n");
    controlleurPince.setPos(-1,30);
    wait(1);
    logger.printf("c 50 \r\n");
    controlleurPince.setPos(-1,-1,50);
    wait(1);
    logger.printf("d 0 \r\n");
    controlleurPince.setPos(-1,0);
    
    while(1);
    
    
    /*wait(1);
    logger.printf("set pos 20 ...\n\r");
    controlleurPince.setPos(20.f,0.f,0.f);
    wait(1);
    logger.printf("set pos 70 ...\n\r");
    controlleurPince.setPos(70.f,0.f,0.f);
    wait(1);
    logger.printf("set pos 20 ...\n\r");
    controlleurPince.setPos(20.f,0.f,0.f);
    logger.printf("Done ...\n\r");*/
    
    /*drapeau = 0;
    init();

    map m(&odo);
    m.addObs(obsCarr (1250, 1000, 220, 220));
    m.addObs(obsCarr (1500, 750, 220, 220));
    m.addObs(obsCarr (1500, 1250, 220, 220));

    init();    
    
    int cote = MAP_RIGHTSIDE;
    /*map m(&odo);
    m.Build(cote);
    m.Execute(1000,1000);
    m.Execute(1500,1000);
    m.Execute(1500,1500);
    m.Execute(110,1000);

    odo.GotoThet(0);
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    logger.printf ("Chemin Fini !");

    while(1);*/
}

void init(void)
{
    logger.baud(9600);
    logger.printf("Hello from main !\n\r");

    init_interrupt();
    goHome();

    SCouleur = VERT;
    LEDV = 1;
    LEDR = 0;

    odo.setPos(110, 1000, 0);
    roboclaw.ResetEnc();
    roboclaw.ForwardM1(0);
    roboclaw.ForwardM2(0);
    wait_ms(500);
    while(1);
    //depart();
    init_interrupt();
    wait_ms(100);
    while(START==0);
    logger.printf("End init\n\r");
}

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