//
// For: https://forum.arduino.cc/t/i-need-help-writing-a-code-that-controls-multiple-servo-motors-using-a-matrix-keypad/993160/
//

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

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, 7, 8, 9 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 2, 3, 4, 5 }; // Pins connected to R1, R2, R3, R4

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

#define NUM_SERVOS 12
Servo myservo[NUM_SERVOS];

const int servoPins[NUM_SERVOS] = { 23, 25, 27, 29, 31, 33, 35, 37, 39, 41, 43, 45 };

void setup() 
{
  Serial.begin(115200);
  Serial.println("Hello, click a number on the keypad");

  for( int i=0; i<NUM_SERVOS; i++)
    myservo[i].attach(servoPins[i]);
}

void loop() 
{
  char key = keypad.getKey();

  if( key != NO_KEY) 
  {
    Serial.print("Key pressed: ");
    Serial.print(key);
    Serial.print("  ");
    // Going to a certain angle
    if( key >= '0' && key <= '9')
    {
      int i = key - '0';
      int a = i * 20;
      Serial.print("Number: ");
      Serial.print(i);
      Serial.print(", angle: ");
      Serial.print(a);
      for( int i=0; i<NUM_SERVOS; i++)
        myservo[i].write(a);
    }
    Serial.println();
  }
}