From 32c74df3fc693782f18d506e9769413bbd41b485 Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Mon, 16 Oct 2023 18:25:18 -0400
Subject: [PATCH] finish wings logic

---
 RELENTLESS/include/wings.h |  4 +++-
 RELENTLESS/src/wings.cpp   | 32 +++++++++++++++++++++++++++++++-
 2 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/RELENTLESS/include/wings.h b/RELENTLESS/include/wings.h
index ac870ba..a1b64d8 100644
--- a/RELENTLESS/include/wings.h
+++ b/RELENTLESS/include/wings.h
@@ -12,9 +12,11 @@ class Wings {
         void toggleLeft(int value);
         void toggleRight(int value);
         void openFor(double duration);
+        int getState();
     
     private:
-        std::uint8_t wingsopen;
+        int left_wing_state;
+        int right_wing_state;
 };
 
 #endif
\ No newline at end of file
diff --git a/RELENTLESS/src/wings.cpp b/RELENTLESS/src/wings.cpp
index 1230c42..22f634f 100644
--- a/RELENTLESS/src/wings.cpp
+++ b/RELENTLESS/src/wings.cpp
@@ -4,25 +4,33 @@
 using namespace globals;
 
 Wings::Wings() {
+    left_wing_state = 0;
+    right_wing_state = 0;
     close();
 };
 
 void Wings::open() {
     left_wing_piston.set_value(1);
+    left_wing_state = 1;
     right_wing_piston.set_value(1);
+    right_wing_state = 1;
 };
 
 void Wings::close() {
     left_wing_piston.set_value(0);
+    left_wing_state = 0;
     right_wing_piston.set_value(0);
+    right_wing_state = 0;
 };
 
 void Wings::toggleLeft(int value) {
-    left_wing_piston.set_value(value);   
+    left_wing_piston.set_value(value);  
+    left_wing_state = value; 
 };
 
 void Wings::toggleRight(int value) {
     right_wing_piston.set_value(value);
+    right_wing_state = value;
 };
 
 void Wings::openFor(double duration) {
@@ -31,5 +39,27 @@ void Wings::openFor(double duration) {
     close();
 };
 
+/*
+    returns int
+    0 -> Both wings closed
+    1 -> Right wing open
+    2 -> Left wing open
+    3 -> Both wings open
+*/
+
+int Wings::getState() {
+    if (left_wing_state == 0 && right_wing_state == 0) {
+        return 0;
+    } else if (left_wing_state == 0 && right_wing_state == 1) {
+        return 1;
+    } else if (left_wing_state == 1 && right_wing_state == 0) {
+        return 2;
+    } else if (left_wing_state == 1 && right_wing_state == 1) {
+        return 3;
+    } else {
+        return 0;
+    }
+}
+