From 04d7b82c0dfd472f69193f8f189cf4688d1eeaac Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Mon, 7 Sep 2020 16:19:55 +0100
Subject: [PATCH] I2C diagnostics

Pending removal of Wire.h
---
 PWMServoDriver.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/PWMServoDriver.cpp b/PWMServoDriver.cpp
index d16783d..ef5b794 100644
--- a/PWMServoDriver.cpp
+++ b/PWMServoDriver.cpp
@@ -77,18 +77,19 @@ bool PWMServoDriver::setup(int board) {
  *  @brief  Sets the PWM output to a servo
  */
 void PWMServoDriver::setServo(byte servoNum, uint16_t value) {
-  DIAG(F("\nsetServo %d %d\n"),servoNum,value);
   int board=servoNum/16; 
   int pin=servoNum%16;
   
   if (setup(board)) {
+    DIAG(F("\nSetServo %d %d\n"),servoNum,value);  
     Wire.beginTransmission(PCA9685_I2C_ADDRESS + board);
     Wire.write(PCA9685_FIRST_SERVO + 4 * pin); // 4 registers per pin
     Wire.write(0);
     Wire.write(0);
     Wire.write(value);
     Wire.write(value >> 8);
-    Wire.endTransmission();
+    byte error=Wire.endTransmission();
+    if (error!=0) DIAG(F("\nSetServo error %d\n"),error); 
   }
 }