From df4a501e8a34fd7748675bf3801be10419334649 Mon Sep 17 00:00:00 2001
From: peteGSX <97784652+peteGSX@users.noreply.github.com>
Date: Mon, 28 Aug 2023 08:36:09 +1000
Subject: [PATCH] Writing to driver

---
 DCCEXParser.cpp | 23 +++++++++++++++--------
 Turntables.cpp  | 22 ++++++----------------
 Turntables.h    |  1 -
 3 files changed, 21 insertions(+), 25 deletions(-)

diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp
index 48c45fb..45e8a73 100644
--- a/DCCEXParser.cpp
+++ b/DCCEXParser.cpp
@@ -1039,19 +1039,27 @@ bool DCCEXParser::parseI(Print *stream, int16_t params, int16_t p[])
         return false;
     
     case 2: // <I id position> - rotate to position for DCC turntables
-        DIAG(F("Rotate DCC turntable %d to position %d"), p[0], p[1]);
+        {
+            Turntable *tto = Turntable::get(p[0]);
+            if (tto) {
+                if (tto->getPosition() == p[1]) return true;
+                uint16_t value = tto->getPositionValue(p[1]);
+                if (value) {
+                    DIAG(F("Rotate DCC turntable %d to position %d"), p[0], p[1]);
+                } else {
+                    return false;
+                }
+            } else {
+                return false;
+            }
+        }
         return true;
 
     case 3: // <I id position activity> rotate to position for EX-Turntable
         {
             Turntable *tto = Turntable::get(p[0]);
             if (tto) {
-                uint16_t value = tto->getPositionValue(p[1]);
-                if (value) {
-                    DIAG(F("Position %d value is %d"), p[1], value);
-                } else {
-                    return false;
-                }
+                if (!tto->setPosition(p[0], p[1], p[2])) return false;
             } else {
                 return false;
             }
@@ -1061,7 +1069,6 @@ bool DCCEXParser::parseI(Print *stream, int16_t params, int16_t p[])
     default:    // If we're here, it must be creating a turntable object
         {
             if (params > 5 && params < 41 && p[1] == HASH_KEYWORD_EXTT) {
-                DIAG(F("Create EXTT turntable %d on vpin %d and address %d with %d positions"), p[0], p[2], p[3], params - 4);
                 if (Turntable::get(p[0])) return false;
                 if (!EXTTTurntable::create(p[0], (VPIN)p[2], (uint8_t)p[3])) return false;
                 Turntable *tto = Turntable::get(p[0]);
diff --git a/Turntables.cpp b/Turntables.cpp
index f75b30d..ec31a00 100644
--- a/Turntables.cpp
+++ b/Turntables.cpp
@@ -84,19 +84,6 @@ uint16_t Turntable::getPositionValue(size_t position) {
 /*
  * Public static functions
  */
-bool Turntable::isPosition(uint16_t id, uint8_t position) {
-  Turntable *tto = get(id);
-  if (tto) {
-    if (tto->getPosition() == position) {
-      return true;
-    } else {
-      return false;
-    }
-  } else {
-    return false;
-  }
-}
-
 bool Turntable::setPositionStateOnly(uint16_t id, uint8_t position) {
   Turntable *tto = get(id);
   if (!tto) return false;
@@ -117,6 +104,7 @@ bool Turntable::setPosition(uint16_t id, uint8_t position, uint8_t activity) {
 
   if (ok) {
     tto->setPositionStateOnly(id, position);
+    tto->_turntableData.position = position;
   }
   return ok;
 }
@@ -163,10 +151,12 @@ EXTTTurntable::EXTTTurntable(uint16_t id, VPIN vpin, uint8_t i2caddress) :
   // EX-Turntable specific code for moving to the specified position
   bool EXTTTurntable::setPositionInternal(uint8_t position, uint8_t activity) {
 #ifndef IO_NO_HAL
-    // Get step value from positions
-    // int value = _exttTurntableData.positions[position];
+    DIAG(F("Set EXTT %d to position %d with activity %d"), _exttTurntableData.vpin, position, activity);
+    // Get position value from position list
+    int16_t value = getPositionValue(position);
+    if (!value) return false; // Return false if it's not a valid position
     // Set position via device driver
-    // EXTurntable::_writeAnalogue(vpin, value, activity, 0);
+    EXTurntable::writeAnalogue(_exttTurntableData.vpin, value, activity);
 #else
     (void)position;
 #endif
diff --git a/Turntables.h b/Turntables.h
index 303c542..f3a3321 100644
--- a/Turntables.h
+++ b/Turntables.h
@@ -159,7 +159,6 @@ public:
    * Public static functions
    */
   inline static bool exists(uint16_t id) { return get(id) != 0; }
-  static bool isPosition(uint16_t id, uint8_t position);
   static bool setPosition(uint16_t id, uint8_t position, uint8_t activity=0);
   static bool setPositionStateOnly(uint16_t id, uint8_t position);
   inline static Turntable *first() { return _firstTurntable; }