From 02c2e979ed3fb39d7eeda4ccd77624e8544f785a Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Wed, 13 Dec 2023 11:18:28 -0500
Subject: [PATCH] enable use of internal pull-ups

---
 projects/etherbotix/main.cpp    | 12 ++---
 projects/etherbotix/user_io.hpp | 82 ++++++++++-----------------------
 2 files changed, 30 insertions(+), 64 deletions(-)

diff --git a/projects/etherbotix/main.cpp b/projects/etherbotix/main.cpp
index 6971209..3823970 100644
--- a/projects/etherbotix/main.cpp
+++ b/projects/etherbotix/main.cpp
@@ -291,16 +291,16 @@ void udp_callback(void *arg, struct udp_pcb *udp, struct pbuf *p,
             {
               // TODO
             }
-            else if (write_addr + j == REG_DIGITAL_OUT)
-            {
-              registers.digital_out = data[i + 6 + j];
-              user_io_set_output();
-            }
             else if (write_addr + j == REG_DIGITAL_DIR)
             {
               registers.digital_dir = data[i + 6 + j];
               user_io_set_direction();
             }
+            else if (write_addr + j == REG_DIGITAL_OUT)
+            {
+              registers.digital_out = data[i + 6 + j];
+              user_io_set_output();
+            }
             else if (write_addr + j == REG_LED)
             {
               registers.led = data[i + 6 + j];
@@ -588,7 +588,7 @@ int main(void)
 
   // Setup register table data
   registers.model_number = 301;  // Arbotix was 300
-  registers.version = 6;
+  registers.version = 7;
   registers.id = 253;
   registers.baud_rate = 1;  // 1mbps
   registers.digital_dir = 0;  // all in
diff --git a/projects/etherbotix/user_io.hpp b/projects/etherbotix/user_io.hpp
index 1dc42d6..91e4d12 100644
--- a/projects/etherbotix/user_io.hpp
+++ b/projects/etherbotix/user_io.hpp
@@ -97,71 +97,37 @@ inline bool user_io_pin_in_use(uint8_t pin)
   return ((user_io_pin_status_ & (1 << pin)) > 0);
 }
 
-inline void user_io_set_output()
+template<typename T, unsigned int pin>
+inline void pin_io_set_output()
 {
-  if (!user_io_pin_in_use(0))
-  {
-    if (registers.digital_out & 0x01)
-      a0_sense::high();
-    else
-      a0_sense::low();
-  }
-
-  if (!user_io_pin_in_use(1))
-  {
-    if (registers.digital_out & 0x02)
-      a1_sense::high();
-    else
-      a1_sense::low();
-  }
-
-  if (!user_io_pin_in_use(2))
-  {
-    if (registers.digital_out & 0x04)
-      a2_sense::high();
-    else
-      a2_sense::low();
-  }
-
-  if (!user_io_pin_in_use(3))
-  {
-    if (registers.digital_out & 0x08)
-      d3::high();
-    else
-      d3::low();
-  }
-
-  if (!user_io_pin_in_use(4))
-  {
-    if (registers.digital_out & 0x10)
-      d4::high();
-    else
-      d4::low();
-  }
-
-  if (!user_io_pin_in_use(5))
+  if (registers.digital_out & (1 << pin))
   {
-    if (registers.digital_out & 0x20)
-      d5::high();
+    // Set to high / pullup
+    if (registers.digital_dir & (1 << pin))
+    {
+      T::high();
+    }
     else
-      d5::low();
+    {
+      T::pullup();
+    }
   }
-
-  if (!user_io_pin_in_use(6))
+  else
   {
-    if (registers.digital_out & 0x40)
-      d6::high();
-    else
-      d6::low();
+    T::low();
   }
+}
 
-  if (!user_io_pin_in_use(7))
-  {
-    if (registers.digital_out & 0x80)
-      d7::high();
-    else
-      d7::low();
-  }
+inline void user_io_set_output()
+{
+  if (!user_io_pin_in_use(0)) pin_io_set_output<a0_sense, 0>();
+  if (!user_io_pin_in_use(1)) pin_io_set_output<a1_sense, 1>();
+  if (!user_io_pin_in_use(2)) pin_io_set_output<a2_sense, 2>();
+  if (!user_io_pin_in_use(3)) pin_io_set_output<d3, 3>();
+  if (!user_io_pin_in_use(4)) pin_io_set_output<d4, 4>();
+  if (!user_io_pin_in_use(5)) pin_io_set_output<d5, 5>();
+  if (!user_io_pin_in_use(6)) pin_io_set_output<d6, 6>();
+  if (!user_io_pin_in_use(7)) pin_io_set_output<d7, 7>();
 }
 
 inline void user_io_set_direction()