#include <popx2.h>

#define STRAING_DELAY 10
#define BACKWARD_DELAY 300
#define WALL_DELAY 100
#define SPEED 30

int TURN_DELAY=350;
float ANGLE = 0;

void FF(){
  /////////// Wall detect /////////////
  while(in(26)){
    if(!in(24)){
      tr(SPEED);
      delay(WALL_DELAY);
    }
    else if(!in(25)){
      tl(SPEED);
      delay(WALL_DELAY);
    }
    else{
      fd2(SPEED, SPEED);
    }
  }
  //////////// Straing alignment ///////////////
  while(in(24) || in(25)){
    if(!in(24)){
      tl(SPEED);
    }
    else if(!in(25)){
      tr(SPEED);
    }
  }
  delay(STRAING_DELAY);
  ao();
  delay(500);
  /////////////////////////////////////////////

  bk(SPEED);
  delay(BACKWARD_DELAY);
  ao();
}

void FR(){
  delay(500);
  sr(SPEED*0.8);
  delay(TURN_DELAY);
  ao();
  delay(500);
}

void FL(){
  delay(500);
  sl(SPEED*0.8);
  delay(TURN_DELAY);
  ao();
  delay(500);
}

void KICK(){
  bk(SPEED);
  delay(BACKWARD_DELAY);
  ao();
  FL();
  FL();
  ao(); delay(500);
  bk(SPEED); delay(BACKWARD_DELAY);
  ao(); delay(500);
  delay(500);servo(1,100);
  delay(500);servo(1,115);
  delay(500);servo(1, 90);
  delay(1000);
}

void setup() {
  servo(1, 90);
  setTextSize(2);
  ///////////////////////////////////

  /* Press OK */
  glcd(0,0,"Press OK");
  while(!sw_OK()){
    TURN_DELAY = map(knob(),0,1024,300,800);
    glcd(1,0,"T=%d",TURN_DELAY);
    delay(100);
  }
  glcd(0,0,"Running ");
  ///////////////////////////////////
  delay(500);
  FF();FL();

  glcd(0,0,"Finished");
}

void loop() {
  //FF();FR();
}