From a19d0c566f57c293646760b831af0abf3525f3fd Mon Sep 17 00:00:00 2001
From: Maximilian Stiefel <stiefel.maximilian@online.de>
Date: Sun, 29 Nov 2020 17:59:19 +0100
Subject: [PATCH] Introduced convenient functions

---
 drivers/a4988.cpp | 35 ++++++++++++++++++++++++++++++++---
 include/a4988.h   | 25 +++++++++++++++++++------
 main.cpp          |  8 ++------
 3 files changed, 53 insertions(+), 15 deletions(-)

diff --git a/drivers/a4988.cpp b/drivers/a4988.cpp
index f38f796..8a891e2 100644
--- a/drivers/a4988.cpp
+++ b/drivers/a4988.cpp
@@ -38,10 +38,20 @@ namespace simon {
           std::this_thread::sleep_for(std::chrono::microseconds(t_sleep));
           gpioHardwarePWM(m_pset->gpio_step, m_pwm_freq, 0);
         }
+        else if (*cmd == "enable") {
+          auto enable = std::static_pointer_cast<c_enable>(cmd);
+          gpioWrite(m_pset->gpio_en, enable->enabled);
+        }
+        else if (*cmd == "kill") {
+          m_alive = false;
+        }
+        else {
+          // TODO: Throw an exception here
+        }
         m_queue.pop();
       }
       else {
-        m_alive = false;
+        // Idle
       }
     }
   }
@@ -49,6 +59,14 @@ namespace simon {
   {
     m_queue.push(cmd);
   }
+  void c_allegro_4988::post_move(const unsigned steps, const bool dir,
+                                 const unsigned period_us)
+  {
+    auto move(std::make_shared<c_move>(steps, dir, period_us));
+    auto cmd = std::static_pointer_cast<c_cmd>(move);
+    m_queue.push(cmd);
+  }
+
   void c_allegro_4988::initialize(void)
   {
     // TODO: proper error management throwing exceptions
@@ -73,15 +91,26 @@ namespace simon {
   int c_allegro_4988::enable(void)
   {
     // TODO: proper error handling
-    gpioWrite(m_pset->gpio_en, 0);
+    auto enable(std::make_shared<c_enable>(true));
+    auto cmd = std::static_pointer_cast<c_cmd>(enable);
+    m_queue.push(cmd);
     return 0;
   }
   int c_allegro_4988::disable(void)
   {
     // TODO: proper error handling
-    gpioWrite(m_pset->gpio_en, 1);
+    auto enable(std::make_shared<c_enable>(false));
+    auto cmd = std::static_pointer_cast<c_cmd>(enable);
+    m_queue.push(cmd);
     return 0;
   }
+  void c_allegro_4988::kill(void)
+  {
+    // TODO: proper error handling
+    auto kill(std::make_shared<c_kill>());
+    auto cmd = std::static_pointer_cast<c_cmd>(kill);
+    m_queue.push(cmd);
+  }
   int c_allegro_4988::set_pwm_freq(const unsigned pwm_freq)
   {
     if (pwm_freq > PI_HW_PWM_MAX_FREQ) {
diff --git a/include/a4988.h b/include/a4988.h
index c17abb8..81e8d67 100644
--- a/include/a4988.h
+++ b/include/a4988.h
@@ -34,17 +34,27 @@ namespace simon {
     class c_move : public c_cmd
     {
       public:
-        unsigned steps;
-        bool dir;
-        unsigned period_us;
-        explicit c_move(void) : c_cmd("move"){}
+        const unsigned steps;
+        const bool dir;
+        const unsigned period_us;
+        c_move(const unsigned isteps, const bool idir,
+                        const unsigned iperiod_us)
+          : c_cmd("move"), steps(isteps), dir(idir), period_us(iperiod_us){}
     };
 
     class c_enable : public c_cmd
     {
       public:
-        bool enabled;
-        explicit c_enable(void) : c_cmd("enable"){}
+        const bool enabled;
+        explicit c_enable(const bool ienabled)
+          : c_cmd("enable"), enabled(ienabled) {}
+    };
+
+    class c_kill : public c_cmd
+    {
+      public:
+        c_kill(void)
+          : c_cmd("kill") {}
     };
 
     class c_allegro_4988
@@ -53,7 +63,10 @@ namespace simon {
         c_allegro_4988(a4988_settings_t* set);
         int enable(void);
         int disable(void);
+        void kill(void);
         void post(std::shared_ptr<c_cmd> cmd);
+        void post_move(const unsigned steps, const bool dir,
+                       const unsigned period_us);
         void run(void);
       private:
         a4988_settings_t* m_pset;
diff --git a/main.cpp b/main.cpp
index cd91606..61946cf 100644
--- a/main.cpp
+++ b/main.cpp
@@ -16,13 +16,9 @@ int main()
     }
   };
   c_allegro_4988 a4988(&a4988_set);
-  auto move(std::make_shared<c_move>());
-  move->steps = 32768;
-  move->dir = true;
-  move->period_us = 1000;
-  auto cmd = std::static_pointer_cast<c_cmd>(move);
   std::thread t1(std::bind(&c_allegro_4988::run, &a4988));
-  a4988.post(cmd);
+  a4988.post_move(32768, true, 100);
+  a4988.kill();
   t1.join();
   std::cout << "Hello camera" << std::endl;
   return 0;