/*
4 e 6: Movimento da base esquerda e direita
2 e 8: Movimento de avanço e recuo        
1 e 7: Movimento para cima e para baixo
3 e 9: Abre a garra / Fecha a garra
A e D: Aumenta a precisão / Diminui a precisão do ângulo do servomotor
*/

#include <Keypad.h> 
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <string.h>
#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define base_minimo 0
#define base_maximo 180
#define atraso 50
#define base 0
#define ombro 1
#define cotovelo 2
#define garra 3

const byte ROWS = 4; // four rows  
const byte COLS = 4; // four columns 
const byte NRO_Servos = 4; // four servos

// define os valores para cada botao do teclado
char hexaKeys[ROWS][COLS]={ {'1','2','3','A'}, 
{'4','5','6','B'}, 
{'7','8','9','C'}, 
{'*','0','#','D'} }; 
// connect to the column pinouts of the keypad 
byte colPins[COLS] = { 16, 17, 5, 18 }; // Pins connected to C1, C2, C3, C4
// connect to the row pinouts of the keypad
byte rowPins[ROWS] = { 15, 2, 0 , 4 }; 
// Pins connected to R1, R2, R3, R4
char texto[16];
// initialize an instance of class NewKeypad
Keypad customKeypad = Keypad( makeKeymap (hexaKeys), rowPins, colPins, ROWS, COLS); 

// Pinos de config servo
int ServoPins[NRO_Servos] = {13, 12, 14, 27 };
Servo servos[NRO_Servos]; 
signed int Servo_angulo[NRO_Servos];
signed int servo_delta_var=10;
bool flag;
void setup(){ 
//  Serial.begin(9600);
  // Init
  lcd.init();
  lcd.backlight();
  for ( int tmp =0; tmp < NRO_Servos; tmp ++){
    servos[tmp].attach( ServoPins[tmp]);
    Servo_angulo[tmp]=90;  
  }
  flag = 1;
}

void loop(){ 
  char valorChave = customKeypad.getKey(); 
  switch (valorChave){
    case '4' : if ( Servo_angulo[base] < base_maximo) {
                flag =1; 
                Servo_angulo[base] += servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case '6' : if (Servo_angulo[base] > base_minimo) {
                flag =1; 
                Servo_angulo[base] -= servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case '2' : if ( Servo_angulo[ombro] < base_maximo) {
                flag =1; 
                Servo_angulo[ombro] += servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[ombro]);*/
              }
              break;
    case '8' : if (Servo_angulo[ombro] > base_minimo) {
                flag =1; 
                Servo_angulo[ombro] -= servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[ombro]);*/
              }
              break;
    case '1' : if ( Servo_angulo[cotovelo] < base_maximo) {
                flag =1; 
                Servo_angulo[cotovelo] += servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case '7' : if (Servo_angulo[cotovelo] > base_minimo) {
                flag =1; 
                Servo_angulo[cotovelo] -= servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case '3' : if ( Servo_angulo[garra] < base_maximo) {
                flag =1; 
                Servo_angulo[garra] += servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case '9' : if (Servo_angulo[garra] > base_minimo) {
                flag =1; 
                Servo_angulo[garra] -= servo_delta_var; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case 'A' : if ( servo_delta_var < 20) { 
                servo_delta_var += 1; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
    case 'D' : if (servo_delta_var > 1) { 
                servo_delta_var -= 1; 
/*                Serial.print(valorChave);
                Serial.print(" : ");
                Serial.println(Servo_angulo[base]);*/
              }
              break;
  }
    // Print something
  if (flag==1){
    lcd.clear();
    lcd.setCursor(14, 0);
    lcd.print("PR");
    lcd.setCursor(14, 1);
    lcd.print("EX");
    lcd.setCursor(0, 0);
    sprintf(texto,"0: %i", Servo_angulo[base]);  
    lcd.print(texto);
    lcd.setCursor(7, 0);
    sprintf(texto,"1: %i", Servo_angulo[ombro]);  
    lcd.print(texto);
    lcd.setCursor(0, 1);
    sprintf(texto,"2: %i", Servo_angulo[cotovelo]);  
    lcd.print(texto);
    lcd.setCursor(7, 1);
    sprintf(texto,"3: %i", Servo_angulo[garra]);  
    lcd.print(texto);
    flag =0;
  }
  for ( int tmp =0; tmp < NRO_Servos; tmp ++){
    servos[tmp].write(Servo_angulo[tmp]);
  }
  delay(atraso);
}