From 611153ce53944e5be90a549526da19ae59e5327f Mon Sep 17 00:00:00 2001
From: ZodiusInfuser <christopher.parrott2@gmail.com>
Date: Fri, 28 Jul 2023 12:53:24 +0100
Subject: [PATCH] Added wrapper functions for controlling Yukon slow IO

---
 examples/yukon/CMakeLists.txt            |  1 +
 examples/yukon/yukon_simple_enable.cmake | 13 +++++++++
 examples/yukon/yukon_simple_enable.cpp   | 32 +++++++++++++++++++++
 libraries/yukon/yukon.cpp                | 36 ++++++++++++++++++++++++
 libraries/yukon/yukon.hpp                | 17 +++++++++++
 5 files changed, 99 insertions(+)
 create mode 100644 examples/yukon/CMakeLists.txt
 create mode 100644 examples/yukon/yukon_simple_enable.cmake
 create mode 100644 examples/yukon/yukon_simple_enable.cpp

diff --git a/examples/yukon/CMakeLists.txt b/examples/yukon/CMakeLists.txt
new file mode 100644
index 00000000..812f98b1
--- /dev/null
+++ b/examples/yukon/CMakeLists.txt
@@ -0,0 +1 @@
+include(yukon_simple_enable.cmake)
\ No newline at end of file
diff --git a/examples/yukon/yukon_simple_enable.cmake b/examples/yukon/yukon_simple_enable.cmake
new file mode 100644
index 00000000..8b1f13a6
--- /dev/null
+++ b/examples/yukon/yukon_simple_enable.cmake
@@ -0,0 +1,13 @@
+set(OUTPUT_NAME yukon_simple_enable)
+add_executable(${OUTPUT_NAME} yukon_simple_enable.cpp)
+
+target_link_libraries(${OUTPUT_NAME}
+        pico_stdlib
+        yukon
+        )
+
+# enable usb output, disable uart output (so it doesn't confuse any connected servos)
+pico_enable_stdio_usb(${OUTPUT_NAME} 1)
+pico_enable_stdio_uart(${OUTPUT_NAME} 0)
+
+pico_add_extra_outputs(${OUTPUT_NAME})
diff --git a/examples/yukon/yukon_simple_enable.cpp b/examples/yukon/yukon_simple_enable.cpp
new file mode 100644
index 00000000..23302077
--- /dev/null
+++ b/examples/yukon/yukon_simple_enable.cpp
@@ -0,0 +1,32 @@
+#include "pico/stdlib.h"
+
+#include "yukon.hpp"
+using namespace pimoroni;
+/*
+Demonstrates how to create a Servo object and control it.
+*/
+
+// How many sweeps of the servo to perform
+const uint SWEEPS = 3;
+
+// The number of discrete sweep steps
+const uint STEPS = 10;
+
+// The time in milliseconds between each step of the sequence
+const uint STEPS_INTERVAL_MS = 500;
+
+// How far from zero to move the servo when sweeping
+constexpr float SWEEP_EXTENT = 90.0f;
+
+
+Yukon y = Yukon();
+
+
+int main() {
+  stdio_init_all();
+
+  // Initialise the servo
+  y.init();
+
+  y.set_slow_output(Yukon::MAIN_EN, true);
+}
diff --git a/libraries/yukon/yukon.cpp b/libraries/yukon/yukon.cpp
index 079d1b15..6bb0ba4d 100644
--- a/libraries/yukon/yukon.cpp
+++ b/libraries/yukon/yukon.cpp
@@ -126,4 +126,40 @@ namespace pimoroni {
     tca1.set_polarity_port(0x0000);
     tca1.set_config_port(0xFCE6);
   }
+
+  TCA9555& Yukon::get_tca_chip(uint chip) {
+    assert(chip < NUM_EXPANDERS);
+    return (chip == 0) ? tca0 : tca1;
+  }
+
+  bool Yukon::get_slow_input(TCA gpio) {
+    return get_tca_chip(gpio.CHIP).get_gpio_input(gpio.GPIO);
+  }
+
+  bool Yukon::get_slow_output(TCA gpio) {
+    return get_tca_chip(gpio.CHIP).get_gpio_output(gpio.GPIO);
+  }
+
+  bool Yukon::get_slow_config(TCA gpio) {
+    return get_tca_chip(gpio.CHIP).get_gpio_config(gpio.GPIO);
+  }
+
+  bool Yukon::get_slow_polarity(TCA gpio) {
+    return get_tca_chip(gpio.CHIP).get_gpio_polarity(gpio.GPIO);
+  }
+
+  void Yukon::set_slow_output(TCA gpio, bool value) {
+    get_tca_chip(gpio.CHIP).set_gpio_output(gpio.GPIO, value);
+  }
+  void Yukon::set_slow_config(TCA gpio, bool output) {
+    get_tca_chip(gpio.CHIP).set_gpio_config(gpio.GPIO, output);
+  }
+
+  void Yukon::set_slow_polarity(TCA gpio, bool polarity) {
+    get_tca_chip(gpio.CHIP).set_gpio_polarity(gpio.GPIO, polarity);
+  }
+
+  void Yukon::change_output_mask(uint8_t chip, uint16_t mask, uint16_t state) {
+    get_tca_chip(chip).change_output_mask(mask, state);
+  }
 }
diff --git a/libraries/yukon/yukon.hpp b/libraries/yukon/yukon.hpp
index 18157e7a..80ca0c74 100644
--- a/libraries/yukon/yukon.hpp
+++ b/libraries/yukon/yukon.hpp
@@ -72,6 +72,8 @@ namespace pimoroni {
     TCA9555 tca0;
     TCA9555 tca1;
 
+    static const uint NUM_EXPANDERS = 2;
+
   public:
     Yukon() :
       i2c(24, 25),
@@ -82,6 +84,21 @@ namespace pimoroni {
 
     void init();
     void reset();
+
+  private:
+    TCA9555& get_tca_chip(uint chip);
+
+  public:
+    bool get_slow_input(TCA gpio);
+    bool get_slow_output(TCA gpio);
+    bool get_slow_config(TCA gpio);
+    bool get_slow_polarity(TCA gpio);
+
+    void set_slow_output(TCA gpio, bool value);
+    void set_slow_config(TCA gpio, bool output);
+    void set_slow_polarity(TCA gpio, bool polarity);
+
+    void change_output_mask(uint8_t chip, uint16_t mask, uint16_t state);
   };
 
 }
\ No newline at end of file