// Programa : Manipulador robótico
#include <math.h>
#include <LiquidCrystal.h>
#include <Adafruit_MotorShield.h>
 
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
 
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
 
 
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
 
int vel = 0;
 
void setup()   
{  
  lcd.begin(16, 2);  
  lcd.setCursor(0,0);  
  lcd.print("Bora Time, Foco!");  
  lcd.setCursor(0,1);  
  lcd.print("            ");    
  AFMS.begin();
  motor1->setSpeed(240);
  motor2->setSpeed(80);
 
}
 
void ufmg(){
  motor1->setSpeed(110);
  motor2->setSpeed(70);
//------------------------------ U -------------------------------------
 
  // desce
  motor2->run(BACKWARD);
  motor1->run(RELEASE);
  delay (2500);
 
  motor2->run(RELEASE);
  delay (500);
 
  // direita
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (2500);
 
  // para
  motor1->run(RELEASE);
  delay (500);
 
  // cima
  motor2->run(FORWARD);
  motor1->run(RELEASE);
  delay (2500);
 
  motor2->run(RELEASE);
  delay (500);
 
  // desce
  motor2->run(BACKWARD);
  motor1->run(RELEASE);
 
  delay (2500);
 
  // para
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay (500);
//---------------------------------------------------------
  // direita
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (800);
//--------------------------- F ------------------------------
 
  // cima
  motor1->run(RELEASE);
  motor2->run(FORWARD);
  delay (1200);
 
  // direita
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (1800);
 
  // para
  motor1->run(RELEASE);
  delay(500);
 
  //esquerda
  motor2->run(RELEASE);
  motor1->run(BACKWARD);
  delay (2200);
 
  // para
  motor1->run(RELEASE);
  delay(1000);
 
  // cima
  motor1->run(RELEASE);
  motor2->run(FORWARD);
  delay (1300);
 
  // direita + espaço
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (3000);
 
//----------------------------- M --------------------------------------------
  // PARA
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(1000);
 
  // desce
  motor2->run(BACKWARD);
  motor1->run(RELEASE);
  delay (2500);
 
  // PARA
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(500);
 
  // Cima
  motor2->run(FORWARD);
  motor1->run(RELEASE);
  delay (2500);
 
  // PARA
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(500);
 
  // desce e direita
  motor2->run(BACKWARD);
  motor1->run(FORWARD);
  delay (1500);
 
  // PARA
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(500);
 
  // Cima e direita
  motor2->run(FORWARD);
  motor1->run(FORWARD);
  delay (1500);
 
  // PARA
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(500);
 
  // desce
  motor2->run(BACKWARD);
  motor1->run(RELEASE);
 
  delay (2500);
 
  // PARA
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(500);
 
//------------------------------------ G -------------------------------------
  // direita (espaço)
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (500);
 
  // Cima
  motor2->run(FORWARD);
  motor1->run(RELEASE);
  delay (2500);
 
  // direita
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (2000);
 
  // PARA
  motor1->run(RELEASE);
  delay(1000);
 
  // Esquerda
  motor2->run(RELEASE);
  motor1->run(BACKWARD);
  delay (2100);
 
  // Para
  motor1->run(RELEASE);
  delay (500);
 
  // Desce 
  motor2->run(BACKWARD);
  motor1->run(RELEASE);
  delay(2500);
 
  // direita
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (2500);
 
  // Cima
  motor2->run(FORWARD);
  motor1->run(RELEASE);
  delay (1200);
 
  // Esquerda
  motor2->run(RELEASE);
  motor1->run(BACKWARD);
  delay (1200);
 
//----------------------------------------------------------------------------
 
  // Para 
  motor2->run(RELEASE);
  motor1->run(RELEASE);
 
}
 
 
 
 
 
 
 
 
void circulo2(){
 
  int vy=20;
  int vx= 200;
 
  motor1->setSpeed(vx);
  motor2->setSpeed(vy);
 
  for (int i = 0;i<10;i++){
    motor1->run(FORWARD);
    motor2->run(FORWARD);
    delay(100);
    vx= vx - 2;
    vy= vy + 5;
    motor1->setSpeed(vx);
    motor2->setSpeed(vy);
  }
 
  for (int i = 0;i<10;i++){
    motor1->run(BACKWARD);
    motor2->run(FORWARD);
    delay(100);
    vx= vx + 2;
    vy= vy - 4;
    motor1->setSpeed(vx);
    motor2->setSpeed(vy);
  }
 
 
  for (int i = 0;i<10;i++){
    motor1->run(BACKWARD);
    motor2->run(BACKWARD);
    delay(100);
    vx= vx - 2;
    vy= vy + 4;
    motor1->setSpeed(vx);
    motor2->setSpeed(vy);
  }
  for (int i = 0;i<10;i++){
    motor1->run(FORWARD);
    motor2->run(BACKWARD);
    delay(100);
    vx= vx + 2;
    vy= vy - 4;
    motor1->setSpeed(vx);
    motor2->setSpeed(vy);
  }
 
  motor1->run(RELEASE);
  motor2->run(RELEASE);
 
}
 
void terceiro(){
  motor1->setSpeed(120);
  motor2->setSpeed(80);
  //------ sobe e direita -----------
 
  motor2->run(FORWARD);
  motor1->run(FORWARD);
  delay (2500);
 
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(1000);
 
//-------- Desce e direita-----------------
 
  motor2->run(BACKWARD); //desce
  motor1->run(FORWARD); //direita
  delay (2700);
 
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(1000);
//-------------- Cima e esquerda------------------
  //motor1->setSpeed(150);
  motor1->setSpeed(140);
  motor2->setSpeed(40);
  //motor1->setSpeed(240);
 
  motor2->run(FORWARD);
  motor1->run(BACKWARD);
  delay (6000);
 
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  delay(1000);
 
//------------- Direita -----------------------
 
  motor2->run(RELEASE);
  motor1->run(FORWARD);
  delay (6500);
 
//------------- Desce e Esquerda -----------------
 
  motor2->run(BACKWARD);
  motor1->run(BACKWARD);
  delay (5500);
 
  motor2->run(RELEASE);
  motor1->run(RELEASE);
  //delay(1000);
 
 
}
 
void loop()  
{  
  int botao;  
  botao = analogRead (0);  //Leitura do valor da porta analógica A0
  //lcd.setCursor(12,1);
  //lcd.print (100+vel);  
 
  if (botao < 100) {  
    //lcd.print ("Direita ");
    ufmg();  
  }  
  else if (botao < 200) {  
    terceiro();
  }  
  else if (botao < 400){  
    delay (300);
  }  
  else if (botao < 600){  
    circulo2 (); 
  }  
  else if (botao < 800){  
    //lcd.print ("Select   ");
    motor1->run(RELEASE);  
  }  
}