#define MOTOR_DIR 0
#define MOTOR_ESQ 1
#define MOTOR_BLOCO 2
#define ENCODER_DIR 0
#define ENCODER_ESQ 1
#define TOQUE_DIR 10
#define TOQUE_ESQ 12
//#define TOQUE_FRENTE TOQUE_DIR || TOQUE_ESQ
#define TOQUE_FRENTE 15
#define BREAK_BEAM 6

#define DIR 0
#define ESQ 1
#use "encoder.ic"

int main(){
    int count, lastCount=0,pidEncoder;
    enable_encoder(ENCODER_DIR);
    enable_encoder(ENCODER_ESQ);
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    start_press();
    //motor(0,60);
    //motor(1,80);
    pidEncoder = start_process(andaReto(60,100,60,100,1));
    while(!stop_button()){
        msleep(1000L);
        count = read_encoder(ENCODER_DIR) + read_encoder(ENCODER_ESQ);
        if(count == lastCount) break; 
        lastCount = count;
    }
    kill_process(pidEncoder);
    ao();
    //start_press();
    if(!stop_button()){
        //motor(0,-60);
        //motor(1,-80);
        pidEncoder = start_process(andaReto(-100,-60,-100,-60,-1));
        msleep(1000L);
        kill_process(pidEncoder);
        ao();
        Rotate(DIR,ROT_90);
        //motor(0,60);
        //motor(1,80);
        pidEncoder = start_process(andaReto(60,100,60,100,1));
        msleep(2000L);
        kill_process(pidEncoder);
        ao();
        Rotate(ESQ,ROT_90);
        //motor(0,60);
        //motor(1,80);
        pidEncoder = start_process(andaReto(60,100,60,100,1));
        msleep(5000L);
        kill_process(pidEncoder);
        ao();
    }
    ao();
    
}