===== linhatp3.ic =====

#define SHAFT1 7
#define SHAFT2  10
#define TEMPO 0.3

#define LEFT_MOTOR 3
#define RIGHT_MOTOR 1

persistent int pgain1, dgain1;
persistent int pgain2, dgain2;

int contador1, contador2;

void Reto(int distancia){
    
    int c1, c2;
    int velocity1, velocity2;
    
    c1 = (int)((float)distancia/1.5);
    c2 = c1;
    
    do{       
        
        encoder();
        
        c1 -= contador1;
        c2 -= contador2;
        
        velocity1 = (int)((float)contador1/TEMPO);
        velocity2 = (int)((float)contador2/TEMPO);
        
        LEFT_POWER = -(pgain1 * (0 - c1) - dgain1 * velocity1);
        RIGHT_POWER = -(pgain2 * (0 - c2) - dgain2 * velocity2);
        
        RIGHT_POWER += (int)((float)RIGHT_POWER*(float)(c2 - c1)/100.0);
        
        if(LEFT_POWER > 100) LEFT_POWER = 100;
        if(RIGHT_POWER > 100) RIGHT_POWER = 100;
        
        LEFT_POWER = (int)((float)LEFT_POWER * 0.3);
        RIGHT_POWER = (int)((float)RIGHT_POWER * 0.3);
        
        motor(LEFT_MOTOR, LEFT_POWER);
        motor(RIGHT_MOTOR, RIGHT_POWER); 
        
    }while((LEFT_POWER > 10) && (RIGHT_POWER > 10) && !stop_button());
    
    ao();
    
}

void ViraDireita(){
    
    int c1, c2;
    int velocity1, velocity2;
    
    c1 = 11;
    c2 = c1;
    
    do{
        
        encoder();
        
        c1 -= contador1;
        c2 += contador2;
        
        velocity1 = (int)((float)contador1/TEMPO);
        velocity2 = -(int)((float)contador2/TEMPO);
        
        LEFT_POWER = -(pgain1 * (0 - c1) - dgain1 * velocity1);
        RIGHT_POWER = pgain2 * (0 - c2) - dgain2 * velocity2;
        
        if(LEFT_POWER > 100) LEFT_POWER = 100;
        if(RIGHT_POWER < -100) RIGHT_POWER = -100;
        
        LEFT_POWER = (int)((float)LEFT_POWER * 0.57);
        RIGHT_POWER = (int)((float)RIGHT_POWER * 0.57);
        
        motor(LEFT_MOTOR, LEFT_POWER);
        motor(RIGHT_MOTOR, RIGHT_POWER); 
        
    }while((LEFT_POWER > 15) && !stop_button());
    
    ao();
    
}

void ViraEsquerda(){
    
    int c1, c2;
    int velocity1, velocity2;
    
    c1 = 12;
    c2 = c1;
    
    do{
        
        encoder();
        
        c1 += contador1;
        c2 -= contador2;
        
        velocity1 = -(int)((float)contador1/TEMPO);
        velocity2 = (int)((float)contador2/TEMPO);
        
        LEFT_POWER = pgain1 * (0 - c1) - dgain1 * velocity1;
        RIGHT_POWER = -(pgain2 * (0 - c2) - dgain2 * velocity2);
        
        if(LEFT_POWER < -100) LEFT_POWER = -100;
        if(RIGHT_POWER > 100) RIGHT_POWER = 100;
        
        LEFT_POWER = (int)((float)LEFT_POWER * 0.57);
        RIGHT_POWER = (int)((float)RIGHT_POWER * 0.57);
        
        motor(LEFT_MOTOR, LEFT_POWER);
        motor(RIGHT_MOTOR, RIGHT_POWER); 
        
    }while((RIGHT_POWER > 15) && !stop_button());
    
    ao();
    
}

void EscolheDistancia(){
    
    // Escolhe a distancia
    while (!start_button()){
        printf ("\nDistancia: %d", knob());
        sleep(0.1);
    }
    
    counts = knob();    
    printf ("\nDistancia: %d", counts);
    sleep(1.0);
    
}

void CalibrarGanhos(){
    
    int temp = knob();
    
    // Calibra o pgain1
    printf ("\nPGAIN ESQ: %d", temp/20);
    while (!start_button()){
        if (temp != knob()){
            temp = knob();
            printf ("\nPGAIN ESQ: %d", temp/20);
        }
    }
    pgain1 = temp/20;
    printf ("\nPGAIN ESQ: %d", pgain1); 
    sleep(1.0);
    
    // Calibra o dgain1
    printf ("\nDGAIN ESQ: %d", knob()/20); 
    while (!start_button()){
        if (temp != knob()){
            temp = knob();
            printf ("\nDGAIN ESQ: %d", temp/20); 
        }
    }
    dgain1 = temp/20;
    printf ("\nDGAIN ESQ: %d", dgain1); 
    sleep(1.0);
    
    // Calibra o pgain2
    printf ("\nPGAIN DIR: %d", knob()/20); 
    while (!start_button()){
        if (temp != knob()){
            temp = knob();
            printf ("\nPGAIN DIR: %d", temp/20); 
        }
    }
    pgain2 = temp/20;
    printf ("\nPGAIN DIR: %d", pgain2); 
    sleep(1.0);    
    
    // Calibra o dgain2
    printf ("\nDGAIN DIR: %d", knob()/20); 
    while (!start_button()){
        if (temp != knob()){
            temp = knob();
            printf ("\nDGAIN DIR: %d", temp/20); 
        }
    }
    dgain2 = temp/20;
    printf ("\nDGAIN DIR: %d", dgain2); 
    sleep(1.0); 
}

void encoder(){
    int ide, idd;
    ide = start_process(pegavalores_esq());
    idd = start_process(pegavalores_dir());
    sleep(TEMPO);
    kill_process(ide);
    kill_process(idd);
    
}

void pegavalores_esq(){
    
    contador1 = 0;
    
    while (1) {
        if (digital(SHAFT1)){
            while (digital(SHAFT1));
            if (LEFT_POWER > 0)
              contador1++;
            else
              contador1--;
        }
        else{
            while (!digital(SHAFT1));
            if (LEFT_POWER > 0)
              contador1++;
            else
              contador1--;
        }
    }
}

void pegavalores_dir(){
    
    contador2 = 0;
    
    while (1) {
        if (digital(SHAFT2)){
            while (digital(SHAFT2));
            if (RIGHT_POWER > 0)
              contador2++;
            else
              contador2--;
        }
        else{
            while (!digital(SHAFT2));
            if (RIGHT_POWER > 0)
              contador2++;
            else
              contador2--;
        }
    }
}