#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--;
      }
  }

}