Programing ears arduino

ino

#include <Servo.h>                    // Incluir la librería Servo
#include <Keypad.h>
Servo servoA1;        // Crear un objeto tipo Servo llamado servo1
Servo servoA2;
Servo servoB1;        // Crear un objeto tipo Servo llamado servo1
Servo servoB2;

char last_key=NO_KEY;

const byte ROWS = 4; // Four rows
const byte COLS = 4; // Three columns
char keys[ROWS][COLS] = {
  {
    'A', 'B', 'C', 'D'  }
  ,
  {
    'E', 'F', 'G', 'H'  }
  ,
  {
    'I', 'J', 'K', 'L'  }
  ,
  {
    'M', 'N', 'O', 'P'  }
  ,
};
// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
byte rowPins[ROWS] = { 
  2, 3, 4, 5 };
// Connect keypad COL0, COL1 and COL2 to these Arduino pins.
byte colPins[COLS] = { 
  7, 8, 12, 13 };

// Create the Keypad
Keypad kpd = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );


int angulo = 0 ;
int buttonState= 0;
char lastButton= '0';

int pos = 0;
int pos2=0;
boolean hasMove = false;
void setup()
{
  Serial.begin(9600);  
  servoA1.attach(6);             // Conectar servo1 al pin 9
  servoA2.attach(9);
  servoB1.attach(10) ;             // Conectar servo1 al pin 9
  servoB2.attach(11);
  servoA1.write(0);
  servoA2.write(0);
  servoB1.write(0);
  servoB2.write(180);

}

void loop()
{
  char key = kpd.getKey();
  if (key!= NO_KEY) {
    Serial.print("changing ");
    Serial.print(last_key);
    Serial.print(" to ");
    Serial.println(key);
    last_key = key;
  }
  Serial.print("the key is ");
  Serial.print(last_key);
  Serial.print("    ");
  Serial.print(key);
  Serial.println("");
  if (key !=NO_KEY) {
  Serial.println("PRESSING");
  }
  if (last_key) {
    switch (last_key)
    {
    case 'A':
      pos = 0;
      pos2=0;
      hasMove=true;
      lastButton='1';
      Serial.println("PRESS 1");
      break;

    case 'B':
      pos = 90;
      pos2=80;
      hasMove=true;
      lastButton='2';
      Serial.println("PRESS 2");
      break;
    case 'C':
      pos = 160;
      pos2=90;
      hasMove=true;
      lastButton='3';
      Serial.println("PRESS 3");
      break;
    case 'D':
      //pos = 90;
      //pos2=160;
      hasMove=false;
      blink();
      lastButton='4';
      Serial.println("PRESS 4");
      break;
          case 'E':
      pos = 0;
      pos2=0;
      hasMove=true;
      lastButton='1';
      Serial.println("PRESS 1");
      break;
    }
    if (hasMove) {
      //delay(500);   

      servoA2.write(pos);
      servoB2.write(180-pos);
      delay(500);

      servoA1.write(pos2);
      servoB1.write(pos2);

      hasMove=false;
      Serial.println(lastButton);
    }
  } 
  else {
    delay(500);
    Serial.print(lastButton);
    Serial.print(" off----");
    Serial.println("");


  }
}

void blink(){

  servoA2.write(90);
  servoB2.write(90);
  int actualPos=0;
  int ar=10;
  int ab=90;
  boolean abajo=true;
  for (int i=0; i<5; i++){
    delay(300);
    if (abajo) {
      servoA1.write(ar);
      servoB1.write(ar);
      abajo=false;
    } else {
      servoA1.write(ab);
      servoB1.write(ab);
      abajo=true;
    
    }
    
    
  }
}