diff --git a/Kode Program Esp32/kamerafix.ino b/Kode Program Esp32/kamerafix.ino
new file mode 100644
index 0000000..dc75dd3
--- /dev/null
+++ b/Kode Program Esp32/kamerafix.ino	
@@ -0,0 +1,256 @@
+#include <Arduino.h>
+#include <WiFi.h>
+#include <WiFiClientSecure.h>
+#include <WiFiManager.h>
+#include "soc/soc.h"
+#include "soc/rtc_cntl_reg.h"
+#include "esp_camera.h"
+#include <UniversalTelegramBot.h>
+#include <ArduinoJson.h>
+
+const char* BOTtoken = "6904712924:AAGEVa4_ejmj-4uKz_nt_i-ceghMPWYW_5M";  // your Bot Token (Get from Botfather)
+String CHAT_ID = "1709517653";  // Chat ID for authorized user
+
+bool sendPhoto = false;
+
+WiFiClientSecure clientTCP;
+UniversalTelegramBot bot(BOTtoken, clientTCP);
+
+#define FLASH_LED_PIN 4
+bool flashState = LOW;
+
+int botRequestDelay = 1000;
+unsigned long lastTimeBotRan;
+
+#define PWDN_GPIO_NUM     32
+#define RESET_GPIO_NUM    -1
+#define XCLK_GPIO_NUM      0
+#define SIOD_GPIO_NUM     26
+#define SIOC_GPIO_NUM     27
+
+#define Y9_GPIO_NUM       35
+#define Y8_GPIO_NUM       34
+#define Y7_GPIO_NUM       39
+#define Y6_GPIO_NUM       36
+#define Y5_GPIO_NUM       21
+#define Y4_GPIO_NUM       19
+#define Y3_GPIO_NUM       18
+#define Y2_GPIO_NUM        5
+#define VSYNC_GPIO_NUM    25
+#define HREF_GPIO_NUM     23
+#define PCLK_GPIO_NUM     22
+
+void configInitCamera() {
+  camera_config_t config;
+  config.ledc_channel = LEDC_CHANNEL_0;
+  config.ledc_timer = LEDC_TIMER_0;
+  config.pin_d0 = Y2_GPIO_NUM;
+  config.pin_d1 = Y3_GPIO_NUM;
+  config.pin_d2 = Y4_GPIO_NUM;
+  config.pin_d3 = Y5_GPIO_NUM;
+  config.pin_d4 = Y6_GPIO_NUM;
+  config.pin_d5 = Y7_GPIO_NUM;
+  config.pin_d6 = Y8_GPIO_NUM;
+  config.pin_d7 = Y9_GPIO_NUM;
+  config.pin_xclk = XCLK_GPIO_NUM;
+  config.pin_pclk = PCLK_GPIO_NUM;
+  config.pin_vsync = VSYNC_GPIO_NUM;
+  config.pin_href = HREF_GPIO_NUM;
+  config.pin_sscb_sda = SIOD_GPIO_NUM;
+  config.pin_sscb_scl = SIOC_GPIO_NUM;
+  config.pin_pwdn = PWDN_GPIO_NUM;
+  config.pin_reset = RESET_GPIO_NUM;
+  config.xclk_freq_hz = 20000000;
+  config.pixel_format = PIXFORMAT_JPEG;
+  config.grab_mode = CAMERA_GRAB_LATEST;
+
+  if (psramFound()) {
+    config.frame_size = FRAMESIZE_UXGA;
+    config.jpeg_quality = 10; //0-63 lower number means higher quality
+    config.fb_count = 1;
+  } else {
+    config.frame_size = FRAMESIZE_SVGA;
+    config.jpeg_quality = 12; //0-63 lower number means higher quality
+    config.fb_count = 1;
+  }
+
+  esp_err_t err = esp_camera_init(&config);
+  if (err != ESP_OK) {
+    Serial.printf("Camera init failed with error 0x%x", err);
+    delay(1000);
+    ESP.restart();
+  }
+}
+
+void handleNewMessages(int numNewMessages) {
+  Serial.print("Handle New Messages: ");
+  Serial.println(numNewMessages);
+
+  for (int i = 0; i < numNewMessages; i++) {
+    String chat_id = String(bot.messages[i].chat_id);
+    if (chat_id != CHAT_ID) {
+      bot.sendMessage(chat_id, "Unauthorized user", "");
+      continue;
+    }
+
+    String text = bot.messages[i].text;
+    Serial.println(text);
+
+    String from_name = bot.messages[i].from_name;
+    if (text == "/start") {
+      String welcome = "Welcome , " + from_name + "\n";
+      welcome += "Use the following commands to interact with the ESP32-CAM \n";
+      welcome += "/photo : takes a new photo\n";
+      welcome += "/flash : toggles flash LED \n";
+      bot.sendMessage(CHAT_ID, welcome, "");
+    }
+    if (text == "/flash") {
+      flashState = !flashState;
+      digitalWrite(FLASH_LED_PIN, flashState);
+      Serial.println("Change flash LED state");
+    }
+    if (text == "/photo") {
+      sendPhoto = true;
+      Serial.println("New photo request");
+      }
+    }
+  }
+
+String sendPhotoTelegram() {
+  const char* myDomain = "api.telegram.org";
+  String getAll = "";
+  String getBody = "";
+
+  // Buang gambar pertama karena kualitas buruk
+  camera_fb_t * fb = NULL;
+  fb = esp_camera_fb_get();
+  esp_camera_fb_return(fb); // buang gambar buffered
+
+  // Ambil foto baru
+  fb = NULL;  
+  fb = esp_camera_fb_get();  
+  if (!fb) {
+    Serial.println("Camera capture failed");
+    delay(1000);
+    ESP.restart();
+    return "Camera capture failed";
+  }  
+  
+  Serial.println("Connect to " + String(myDomain));
+
+  if (clientTCP.connect(myDomain, 443)) {
+    Serial.println("Connection successful");
+    
+    String head = "--kwhbot\r\n"
+                  "Content-Disposition: form-data; name=\"chat_id\"; \r\n\r\n" + CHAT_ID + 
+                  "\r\n--kwhbot\r\n"
+                  "Content-Disposition: form-data; name=\"photo\"; filename=\"esp32-cam.jpg\"\r\n"
+                  "Content-Type: image/jpeg\r\n\r\n";
+    String tail = "\r\n--kwhbot--\r\n";
+
+    size_t imageLen = fb->len;
+    size_t extraLen = head.length() + tail.length();
+    size_t totalLen = imageLen + extraLen;
+  
+    clientTCP.println("POST /bot" + String(BOTtoken) + "/sendPhoto HTTP/1.1");
+    clientTCP.println("Host: " + String(myDomain));
+    clientTCP.println("Content-Length: " + String(totalLen));
+    clientTCP.println("Content-Type: multipart/form-data; boundary=kwhbot");
+    clientTCP.println();
+    clientTCP.print(head);
+
+    uint8_t *fbBuf = fb->buf;
+    size_t fbLen = fb->len;
+    for (size_t n = 0; n < fbLen; n = n + 1024) {
+      if (n + 1024 < fbLen) {
+        clientTCP.write(fbBuf, 1024);
+        fbBuf += 1024;
+      } else if (fbLen % 1024 > 0) {
+        size_t remainder = fbLen % 1024;
+        clientTCP.write(fbBuf, remainder);
+      }
+    }  
+
+    clientTCP.print(tail);
+    esp_camera_fb_return(fb);
+    
+    int waitTime = 10000;   // timeout 10 detik
+    long startTimer = millis();
+    boolean state = false;
+    
+    while ((startTimer + waitTime) > millis()) {
+      Serial.print(".");
+      delay(100);      
+      while (clientTCP.available()) {
+        char c = clientTCP.read();
+        if (state == true) getBody += String(c);        
+        if (c == '\n') {
+          if (getAll.length() == 0) state = true; 
+          getAll = "";
+        } else if (c != '\r') {
+          getAll += String(c);
+        }
+        startTimer = millis();
+      }
+      if (getBody.length() > 0) break;
+    }
+    clientTCP.stop();
+    Serial.println(getBody);
+  } else {
+    getBody = "Connected to api.telegram.org failed.";
+    Serial.println("Connected to api.telegram.org failed.");
+  }
+  return getBody;
+}
+
+
+void setup() {
+  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
+  Serial.begin(115200);
+
+  pinMode(FLASH_LED_PIN, OUTPUT);
+  digitalWrite(FLASH_LED_PIN, flashState);
+
+  configInitCamera();
+
+  WiFiManager wifiManager;
+
+  wifiManager.setConfigPortalTimeout(180); // Set timeout portal ke 3 menit
+
+  // Coba terhubung ke Wi-Fi
+  if (!wifiManager.autoConnect("CAM-KWH","tugasakhir")) {
+    Serial.println("Failed to connect to WiFi, entering config mode");
+    // Jika gagal terhubung ke Wi-Fi, masuk ke mode konfigurasi
+    wifiManager.startConfigPortal("AutoConnectAP");
+  }
+
+  // Periksa koneksi WiFi
+  if (WiFi.status() != WL_CONNECTED) {
+    Serial.println("Failed to connect to WiFi");
+    return;
+  } else {
+    Serial.println("Connected to WiFi");
+  }
+
+  clientTCP.setCACert(TELEGRAM_CERTIFICATE_ROOT);
+
+  Serial.print("ESP32-CAM IP Address: ");
+  Serial.println(WiFi.localIP());
+}
+
+void loop() {
+  if (sendPhoto) {
+    Serial.println("Preparing photo");
+    sendPhotoTelegram();
+    sendPhoto = false;
+  }
+  if (millis() > lastTimeBotRan + botRequestDelay) {
+    int numNewMessages = bot.getUpdates(bot.last_message_received + 1);
+    while (numNewMessages) {
+      Serial.println("got response");
+      handleNewMessages(numNewMessages);
+      numNewMessages = bot.getUpdates(bot.last_message_received + 1);
+    }
+    lastTimeBotRan = millis();
+  }
+}
diff --git a/Kode Program Esp32/progamesp32fix.ino b/Kode Program Esp32/progamesp32fix.ino
new file mode 100644
index 0000000..5182b8e
--- /dev/null
+++ b/Kode Program Esp32/progamesp32fix.ino	
@@ -0,0 +1,378 @@
+#include <Arduino.h>
+#include <WiFi.h>
+#include <Firebase_ESP_Client.h>
+#include <Wire.h>
+#include <Adafruit_PWMServoDriver.h>
+#include <PZEM004Tv30.h>
+#include <WiFiManager.h>
+#include "addons/TokenHelper.h"
+#include <NTPClient.h>
+#include <WiFiUdp.h>
+
+// Firebase Credentials
+#define API_KEY "AIzaSyCfdJ7v2WdONyDVSgpQQCyzi__tvklcxIk"
+#define DATABASE_URL "https://inputkwh-b45d4-default-rtdb.asia-southeast1.firebasedatabase.app/"
+#define USER_EMAIL "nurilakbar23@gmail.com"
+#define USER_PASSWORD "akugaktau"
+
+// Relay Pins
+const int relay1Pin = 5;
+const int relay2Pin = 18;
+const int relay3Pin = 19;
+const int relay4Pin = 23;
+
+// Servo Parameters
+#define SERVOMAX  600 // 60 degrees
+#define SERVOMIN  500 // 40 degrees
+#define STEPS 20      // Number of steps to slow down servo movement
+
+// Define servo positions for keypad (4x3 matrix)
+const int servoPins[4][3] = {
+  {0, 1, 2},
+  {3, 4, 5},
+  {6, 7, 8},
+  {9, 10, 11}
+};
+
+// Define keypad layout
+const char keypad[4][3] = {
+  {'1', '2', '3'},
+  {'4', '5', '6'},
+  {'7', '8', '9'},
+  {'*', '0', '#'}
+};
+
+// Constants for delay
+const int SERVO_DELAY = 20; // Delay between each servo step
+const int BUTTON_DELAY = 500; // Delay between button press
+
+FirebaseData fbdoChannels;
+FirebaseData fbdoTokens;
+FirebaseData fbdo;
+FirebaseAuth auth;
+FirebaseConfig config;
+Adafruit_PWMServoDriver pca9685 = Adafruit_PWMServoDriver();
+
+String previousTokens = ""; // Menyimpan token sebelumnya
+
+// Declare PZEM sensor object
+#define RXD2 16
+#define TXD2 17
+HardwareSerial SerialPZEM(2); // Create HardwareSerial object for Serial2
+PZEM004Tv30 pzem(SerialPZEM, RXD2, TXD2); // Initialize PZEM004Tv30 object
+
+unsigned long sendDataPrevMillis = 0;
+unsigned long lastDataSentMillis = 0;
+bool signupOK = false;
+
+WiFiUDP ntpUDP;
+NTPClient timeClient(ntpUDP, "pool.ntp.org", 25200); // NTPClient object for WIB timezone (UTC+7)
+
+void channelStreamCallback(FirebaseStream data) {
+  Serial.printf("Channel stream path: %s\n", data.streamPath().c_str());
+  Serial.printf("Event path: %s\n", data.dataPath().c_str());
+  Serial.printf("Data type: %s\n", data.dataType().c_str());
+  Serial.printf("Event type: %s\n", data.eventType().c_str());
+  Serial.printf("Data: %s\n", data.stringData().c_str());
+
+  String jsonData = data.stringData();
+  Serial.printf("Received channel data: %s\n", jsonData.c_str());
+
+  // Parse JSON
+  FirebaseJson json;
+  json.setJsonData(jsonData);
+  FirebaseJsonData result;
+
+  // Process each channel
+  if (json.get(result, "channel1")) {
+    String channel1Value = result.stringValue;
+    controlRelay(relay1Pin, channel1Value);
+  }
+
+  if (json.get(result, "channel2")) {
+    String channel2Value = result.stringValue;
+    controlRelay(relay2Pin, channel2Value);
+  }
+
+  if (json.get(result, "channel3")) {
+    String channel3Value = result.stringValue;
+    controlRelay(relay3Pin, channel3Value);
+  }
+
+  if (json.get(result, "channel4")) {
+    String channel4Value = result.stringValue;
+    controlRelay(relay4Pin, channel4Value);
+  }
+}
+
+void tokenStreamCallback(FirebaseStream data) {
+  Serial.printf("Token stream path: %s\n", data.streamPath().c_str());
+  Serial.printf("Event path: %s\n", data.dataPath().c_str());
+  Serial.printf("Data type: %s\n", data.dataType().c_str());
+  Serial.printf("Event type: %s\n", data.eventType().c_str());
+  Serial.printf("Data: %s\n", data.stringData().c_str());
+
+  String tokens = data.stringData();
+  Serial.printf("Received tokens: %s\n", tokens.c_str());
+  Serial.printf("Tokens length: %d\n", tokens.length());
+  processTokens(tokens);
+}
+
+void streamTimeoutCallback(bool timeout) {
+  if (timeout)
+    Serial.println("Stream timeout, resuming...");
+  else
+    Serial.println("Stream resumed");
+}
+
+void setup() {
+  Serial.begin(115200);
+
+  // Inisialisasi pin relay sebagai output
+  pinMode(relay1Pin, OUTPUT);
+  pinMode(relay2Pin, OUTPUT);
+  pinMode(relay3Pin, OUTPUT);
+  pinMode(relay4Pin, OUTPUT);
+
+  // Inisialisasi relay ke posisi mati (off)
+  digitalWrite(relay1Pin, HIGH);
+  digitalWrite(relay2Pin, HIGH);
+  digitalWrite(relay3Pin, HIGH);
+  digitalWrite(relay4Pin, HIGH);
+
+  WiFiManager wifiManager;
+
+  wifiManager.setConfigPortalTimeout(180); // Set timeout portal ke 3 menit
+
+  // Coba terhubung ke Wi-Fi
+  if (!wifiManager.autoConnect("MAIN-KWH","tugasakhir")) {
+    Serial.println("Failed to connect to WiFi, entering config mode");
+    // Jika gagal terhubung ke Wi-Fi, masuk ke mode konfigurasi
+    wifiManager.startConfigPortal("AutoConnectAP");
+  }
+
+  // Periksa koneksi WiFi
+  if (WiFi.status() != WL_CONNECTED) {
+    Serial.println("Failed to connect to WiFi");
+    return;
+  } else {
+    Serial.println("Connected to WiFi");
+  }
+
+  config.api_key = API_KEY;
+  config.database_url = DATABASE_URL;
+  config.token_status_callback = tokenStatusCallback; // Callback status token
+
+  auth.user.email = USER_EMAIL;
+  auth.user.password = USER_PASSWORD;
+
+  // Mendaftar (Sign up)
+  if (Firebase.signUp(&config, &auth, "", "")) {
+    Serial.println("Sign up successful");
+    signupOK = true;
+  } else {
+    Serial.println("Sign up failed");
+    Serial.println(config.signer.signupError.message.c_str());
+  }
+
+  // Mulai koneksi Firebase
+  Firebase.begin(&config, &auth);
+  Firebase.reconnectNetwork(true);
+
+  // Mulai stream Firebase untuk channel
+  if (!Firebase.RTDB.beginStream(&fbdoChannels, "/channels")) {
+    Serial.printf("Failed to start channels stream, %s\n", fbdoChannels.errorReason().c_str());
+  } else {
+    Serial.println("Channels stream started successfully!");
+    Firebase.RTDB.setStreamCallback(&fbdoChannels, channelStreamCallback, streamTimeoutCallback);
+  }
+
+  // Mulai stream Firebase untuk token
+  if (!Firebase.RTDB.beginStream(&fbdoTokens, "/tokens")) {
+    Serial.printf("Failed to start tokens stream, %s\n", fbdoTokens.errorReason().c_str());
+  } else {
+    Serial.println("Tokens stream started successfully!");
+    Firebase.RTDB.setStreamCallback(&fbdoTokens, tokenStreamCallback, streamTimeoutCallback);
+  }
+
+  // Setup PCA9685
+  pca9685.begin();
+  pca9685.setPWMFreq(60); // Analog servos work at ~60 Hz
+  delay(10);
+
+  // Pindahkan semua servos ke posisi awal (60 derajat)
+  for (int i = 0; i < 12; i++) {
+    pca9685.setPWM(i, 0, SERVOMAX);
+  }
+
+  // Set posisi awal untuk servos '1', '4', '9', dan '#' tanpa bergerak
+  setInitialPosition('1');
+  setInitialPosition('4');
+  setInitialPosition('9');
+  setInitialPosition('#');
+
+  // Inisialisasi NTP client
+  timeClient.begin();
+  timeClient.update();
+}
+
+
+void loop() {
+  // Periksa koneksi WiFi dan kesiapan Firebase
+  if (WiFi.status() == WL_CONNECTED && Firebase.ready() && signupOK) {
+    unsigned long currentMillis = millis();
+
+    // Periksa apakah sudah waktunya mengirim data sensor
+    if (currentMillis - sendDataPrevMillis >= 1000) { // Kirim data setiap detik
+      sendDataPrevMillis = currentMillis;
+
+      // Baca data dari sensor PZEM
+      float voltage = pzem.voltage();
+      float current = pzem.current();
+      float power = pzem.power();
+      float energy = pzem.energy();
+      float frequency = pzem.frequency();
+      float pf = pzem.pf();
+      float va = voltage * current; // Menghitung daya semu (VA)
+
+      // Kirim data ke Firebase RTDB untuk path "sensor" (tidak diubah)
+      if (Firebase.RTDB.setFloat(&fbdo, "sensor/voltage", voltage) &&
+          Firebase.RTDB.setFloat(&fbdo, "sensor/current", current) &&
+          Firebase.RTDB.setFloat(&fbdo, "sensor/power", power) &&
+          Firebase.RTDB.setFloat(&fbdo, "sensor/energy", energy) &&
+          Firebase.RTDB.setFloat(&fbdo, "sensor/frequency", frequency) &&
+          Firebase.RTDB.setFloat(&fbdo, "sensor/pf", pf) &&
+          Firebase.RTDB.setFloat(&fbdo, "sensor/va", va)) {
+        Serial.println("Data sent successfully to sensor");
+      } else {
+        Serial.println("Failed to send data to sensor");
+        Serial.println(fbdo.errorReason()); // Tampilkan alasan kegagalan
+      }
+    }
+
+    // Periksa apakah sudah waktunya mengirim data-listrik
+    if (currentMillis - lastDataSentMillis >= 60000) { // Kirim data setiap menit (60,000 milidetik)
+      lastDataSentMillis = currentMillis;
+
+      // Update NTP client to get current time
+      timeClient.update();
+
+      // Get formatted date and time
+      String formattedTime = timeClient.getFormattedTime();
+      time_t rawTime = timeClient.getEpochTime();
+      struct tm *timeInfo = localtime(&rawTime);
+      String year = String(timeInfo->tm_year + 1900);
+      String month = String(timeInfo->tm_mon + 1);
+      String day = String(timeInfo->tm_mday);
+      String hour = formattedTime.substring(0, 2);
+      String minute = formattedTime.substring(3, 5);
+
+      // Combine date and time into a single field
+      String dateTime = day + "/" + month + "/" + year + " " + hour + ":" + minute;
+
+      // Buat objek JSON untuk data-listrik
+      FirebaseJson json;
+      json.set("dateTime", dateTime);
+      json.set("voltage", pzem.voltage());
+      json.set("current", pzem.current());
+      json.set("power", pzem.power());
+      json.set("energy", pzem.energy());
+      json.set("frequency", pzem.frequency());
+      json.set("pf", pzem.pf());
+      json.set("va", pzem.voltage() * pzem.current()); // Menambahkan data VA ke objek JSON
+
+      // Buat key baru dengan menggunakan timestamp
+      String timestamp = String(rawTime);
+
+      // Kirim data ke Firebase RTDB menggunakan push untuk menambahkan data baru di path "data-listrik" dengan key timestamp
+      if (Firebase.RTDB.setJSON(&fbdo, "data-listrik/" + timestamp, &json)) {
+        Serial.println("Data sent successfully to data-listrik");
+      } else {
+        Serial.println("Failed to send data to data-listrik");
+        Serial.println(fbdo.errorReason()); // Tampilkan alasan kegagalan
+      }
+    }
+  } else {
+    // Reconnect WiFi and Firebase if not connected
+    WiFi.reconnect();
+    Firebase.reconnectWiFi(true);
+    delay(2000);
+  }
+  delay(10);  // Add a short delay to prevent WDT reset
+}
+
+void controlRelay(int relayPin, String state) {
+  if (state == "On") {
+    digitalWrite(relayPin, LOW); // Turn relay on
+  } else {
+    digitalWrite(relayPin, HIGH); // Turn relay off
+  }
+}
+
+void processTokens(String tokens) {
+  if (tokens.length() == 20) { // Jika panjang token adalah 20 karakter
+    for (int i = 0; i < 20; i++) {
+      char digit = tokens.charAt(i);
+      pressKey(digit); // Tekan tombol sesuai digit token
+      delay(BUTTON_DELAY); // Sesuaikan penundaan jika diperlukan agar keypad mendeteksi tekanan tombol
+    }
+    
+    // Tekan tombol '#' setelah mengeksekusi 20 digit
+    pressKey('#');
+    delay(BUTTON_DELAY); // Tunda sebelum membaca token lagi
+  } else {
+    Serial.println("Invalid token length"); // Tampilkan pesan kesalahan jika panjang token tidak valid
+  }
+}
+
+void pressKey(char key) {
+  int startPos, endPos;
+  
+  if (key == '1' || key == '4' || key == '9' || key == '#') {
+    startPos = SERVOMIN;  // 40 degrees
+    endPos = SERVOMAX - 20;    // 60 degrees, kurangi 50 agar gerakan lebih kecil
+  } else {
+    startPos = SERVOMAX;  // 60 degrees
+    endPos = SERVOMIN + 20;    // 40 degrees, tambahkan 50 agar gerakan lebih kecil
+  }
+
+  for (int row = 0; row < 4; row++) {
+    for (int col = 0; col < 3; col++) {
+      if (keypad[row][col] == key) {
+        int servoNum = servoPins[row][col];
+        
+        // Move servo from startPos to endPos with steps
+        for (int pulse = startPos; (startPos < endPos) ? (pulse <= endPos) : (pulse >= endPos); pulse += ((startPos < endPos) ? 1 : -1) * ((abs(endPos - startPos)) / STEPS)) {
+          pca9685.setPWM(servoNum, 0, pulse);
+          delay(SERVO_DELAY); // Delay to slow down servo movement
+        }
+
+        delay(BUTTON_DELAY); // Delay to simulate button press
+        
+        // Return servo from endPos to startPos with steps
+        for (int pulse = endPos; (endPos < startPos) ? (pulse <= startPos) : (pulse >= startPos); pulse += ((endPos < startPos) ? 1 : -1) * ((abs(startPos - endPos)) / STEPS)) {
+          pca9685.setPWM(servoNum, 0, pulse);
+          delay(SERVO_DELAY); // Delay to slow down servo movement
+        }
+        
+        delay(BUTTON_DELAY); // Delay after button press
+        
+        return;
+      }
+    }
+  }
+  Serial.printf("Button %c not found\n", key);
+}
+
+void setInitialPosition(char key) {
+  for (int row = 0; row < 4; row++) {
+    for (int col = 0; col < 3; col++) {
+      if (keypad[row][col] == key) {
+        int servoNum = servoPins[row][col];
+        pca9685.setPWM(servoNum, 0, SERVOMIN); // Set to 40 degrees
+        return;
+      }
+    }
+  }
+  Serial.printf("Button %c not found\n", key);
+}