#include <Keypad.h>
#include <Servo.h>

//// Configuracion de pad
/////////////////////////

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

uint8_t colPins[COLS] = { 6, 5, 4, 2 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 9, 8, 7, 3 }; // Pins connected to R1, R2, R3, R4

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

//// Configuracion de servo
///////////////////////////

Servo myServo;
Servo myServo2;

int rMin = 0;
int rMax = 180;

int step = 15;

void setup() {
  myServo.attach(11);
  myServo2.attach(10);
}

void loop() {
  char direction = keypad.getKey();
  moveToStep(direction);
}

void moveToStep(char isWord) {
  switch (isWord) {
    case '2':
      myServo.write(getNewAngule(myServo, false));
      break;
    case '8':
      myServo.write(getNewAngule(myServo, true));
      break;
    case '4':
      myServo2.write(getNewAngule(myServo2, false));
      break;
    case '6':
      myServo2.write(getNewAngule(myServo2, true));
      break;
  }
}

int getNewAngule(Servo theServo, bool more) {
  int angule = theServo.read();
  
  if (angule <= 0 && !more) {
    return 0;
  }

  if(angule >= 180 && more){
    return 180;
  }

  int newAngule;

  if (more) {
    newAngule = angule + step;
  } else {
    newAngule = angule - step;
  }

  return newAngule;
}

void moveAll(char isWord) {
  switch (isWord) {
    case '2':
      myServo.write(rMin);
      break;
    case '8':
      myServo.write(rMax);
      break;
    case '4':
      myServo2.write(rMin);
      break;
    case '6':
      myServo2.write(rMax);
      break;
  }
}