From 4c33a751643ecf282e0a4c611e514aaa4fe5fa15 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Fri, 28 Mar 2025 23:46:25 +0100
Subject: [PATCH] =?UTF-8?q?=E2=9C=8D=EF=B8=8F=20Adds=20bulk=20writing=20of?=
 =?UTF-8?q?=20pwm=20values=20to=20PCA9685?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../peripherals/servo_controller.h            | 21 ++++++++++++++++---
 1 file changed, 18 insertions(+), 3 deletions(-)

diff --git a/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h b/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h
index 930e01c..a31bb38 100644
--- a/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h
+++ b/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h
@@ -8,6 +8,7 @@
 #include <template/stateful_endpoint.h>
 #include <utils/math_utils.h>
 #include <settings/servo_settings.h>
+#include <Wire.h>
 
 /*
  * Servo Settings
@@ -52,6 +53,18 @@ class ServoController : public StatefulService<ServoSettings> {
         _pca.setPWM(index, 0, value);
     }
 
+    void writeMany(uint8_t count, uint16_t *values) {
+        Wire.beginTransmission(PCA9685_I2C_ADDRESS);
+        Wire.write(0x06);
+        for (int i = 0; i < count; i++) {
+            Wire.write(0);
+            Wire.write(0);
+            Wire.write(values[i] & 0xFF);
+            Wire.write(values[i] >> 8);
+        }
+        Wire.endTransmission();
+    }
+
     void activate() {
         if (is_active) return;
         control_state = SERVO_CONTROL_STATE::ANGLE;
@@ -98,17 +111,19 @@ class ServoController : public StatefulService<ServoSettings> {
     }
 
     void calculatePWM() {
+        uint16_t pwms[12];
         for (int i = 0; i < 12; i++) {
-            angles[i] = lerp(angles[i], target_angles[i], 0.2);
+            angles[i] = lerp(angles[i], target_angles[i], 0.1);
             auto &servo = state().servos[i];
             float angle = servo.direction * angles[i] + servo.centerAngle;
             uint16_t pwm = angle * servo.conversion + servo.centerPwm;
-            if (pwm < 105 || pwm > 600) {
+            if (pwm < 125 || pwm > 600) {
                 ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
                 continue;
             }
-            pcaWrite(i, pwm);
+            pwms[i] = pwm;
         }
+        writeMany(12, pwms);
     }
 
     void updateServoState() {