From 964f39d3765ff49541b9781f4552beaab8ab6bfb Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Thu, 23 Jul 2020 12:48:38 +0100
Subject: [PATCH] Fix hanging turnouts

Caused by tt->activate call when Turnouts was a struct not a class!
---
 PWMServoDriver.cpp | 39 ++++++++++++++++++++++++++-------------
 PWMServoDriver.h   |  3 ++-
 Turnouts.cpp       |  6 ++++--
 Turnouts.h         |  5 +++--
 4 files changed, 35 insertions(+), 18 deletions(-)

diff --git a/PWMServoDriver.cpp b/PWMServoDriver.cpp
index 75af8e1..d16783d 100644
--- a/PWMServoDriver.cpp
+++ b/PWMServoDriver.cpp
@@ -46,37 +46,50 @@ const uint8_t PRESCALE_50HZ = (uint8_t)(((FREQUENCY_OSCILLATOR / (50.0 * 4096.0)
  */
 
 byte PWMServoDriver::setupFlags=0;  // boards that have been initialised
+byte PWMServoDriver::failFlags=0;  // boards that have faild initialisation
 
-void PWMServoDriver::setup(int board) {
-  if (board>3 || setupFlags & (1<<board)) return;
+bool PWMServoDriver::setup(int board) {
+  if (board>3 || (failFlags & (1<<board))) return false; 
+  if (setupFlags & (1<<board)) return true; 
+  
   Wire.begin();
   uint8_t i2caddr=PCA9685_I2C_ADDRESS + board;
+
+  // Terst if device is available
+  Wire.beginTransmission(i2caddr);
+  byte error = Wire.endTransmission();
+  if (error!=0) {
+    DIAG(F("\nI2C Servo device 0x%x Not Found %d\n"),i2caddr, error);
+    failFlags|=1<<board;  
+    return false;
+  }
+    
   //DIAG(F("\nPWMServoDriver::setup %x prescale=%d"),i2caddr,PRESCALE_50HZ); 
   writeRegister(i2caddr,PCA9685_MODE1, MODE1_SLEEP | MODE1_AI);    
   writeRegister(i2caddr,PCA9685_PRESCALE, PRESCALE_50HZ);  
   writeRegister(i2caddr,PCA9685_MODE1,MODE1_AI);
   writeRegister(i2caddr,PCA9685_MODE1,  MODE1_RESTART | MODE1_AI);
   setupFlags|=1<<board;
+  return true;
 }
 
 /*!
  *  @brief  Sets the PWM output to a servo
  */
 void PWMServoDriver::setServo(byte servoNum, uint16_t value) {
-  //DIAG(F("\nsetServo %d %d\n"),servoNum,value);
+  DIAG(F("\nsetServo %d %d\n"),servoNum,value);
   int board=servoNum/16; 
   int pin=servoNum%16;
   
-  if ( board>3) return; // safe dropout
-  setup(board); // in case not already done
-  
-  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();
+  if (setup(board)) {
+    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();
+  }
 }
 
 void PWMServoDriver::writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d) {
diff --git a/PWMServoDriver.h b/PWMServoDriver.h
index da27bac..6bcbae7 100644
--- a/PWMServoDriver.h
+++ b/PWMServoDriver.h
@@ -13,7 +13,8 @@ public:
     
 private:
   static byte setupFlags; 
-  static void setup(int board);
+  static byte failFlags; 
+  static bool setup(int board);
   static void writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d);
 };
 
diff --git a/Turnouts.cpp b/Turnouts.cpp
index f280f86..2eec241 100644
--- a/Turnouts.cpp
+++ b/Turnouts.cpp
@@ -23,6 +23,7 @@
 #include "PWMServoDriver.h"
 
  bool Turnout::activate(int n,bool state){
+  //DIAG(F("\nTurnout::activate(%d,%d)\n"),n,state);
   Turnout * tt=get(n);
   if (tt==NULL) return false;
   tt->activate(state);
@@ -31,10 +32,11 @@
 }
 
 // activate is virtual here so that it can be overridden by a non-DCC turnout mechanism
-void Turnout::activate(bool state) {
+ void Turnout::activate(bool state) {
+  //DIAG(F("\nTurnout::activate\n"));
   if (state) data.tStatus|=STATUS_ACTIVE;
   else data.tStatus &= ~STATUS_ACTIVE;                            
-  if (data.tStatus & STATUS_PWM) PWMServoDriver::setServo(data.tStatus & STATUS_PWMPIN, (data.inactiveAngle+state?data.moveAngle:0));
+  if (data.tStatus & STATUS_PWM) PWMServoDriver::setServo(data.tStatus & STATUS_PWMPIN, (data.inactiveAngle+(state?data.moveAngle:0)));
      else DCC::setAccessory(data.address,data.subAddress, state);
 }
 ///////////////////////////////////////////////////////////////////////////////
diff --git a/Turnouts.h b/Turnouts.h
index 7fbbbe0..7a2edf0 100644
--- a/Turnouts.h
+++ b/Turnouts.h
@@ -33,7 +33,8 @@ struct TurnoutData {
    union {int address; int inactiveAngle;}; // DCC address or PWM servo angle 
 };
 
-struct Turnout{
+class Turnout {
+  public:
   static Turnout *firstTurnout;
   TurnoutData data;
   Turnout *nextTurnout;
@@ -47,7 +48,7 @@ struct Turnout{
   static Turnout *create(int id);
   static void show(Print & stream, int n);
   static bool showAll(Print & stream);
-  virtual void activate(bool state);
+  void activate(bool state);
 }; // Turnout
   
 #endif