From 8a8d40af6f9ee8bbfd5f2e1b8a05e745f3f6eb4a Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Fri, 1 Dec 2023 08:39:28 -0500
Subject: [PATCH] reimplement wings class

---
 CLOUDS/include/Wings.h | 22 ++++++++++++
 CLOUDS/src/Wings.cpp   | 76 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 98 insertions(+)
 create mode 100644 CLOUDS/include/Wings.h
 create mode 100644 CLOUDS/src/Wings.cpp

diff --git a/CLOUDS/include/Wings.h b/CLOUDS/include/Wings.h
new file mode 100644
index 0000000..a1b64d8
--- /dev/null
+++ b/CLOUDS/include/Wings.h
@@ -0,0 +1,22 @@
+#pragma once
+#ifndef _Wings_h
+#define _Wings_h_
+
+#include "main.h"
+
+class Wings {
+    public:
+        Wings();
+        void open();
+        void close();
+        void toggleLeft(int value);
+        void toggleRight(int value);
+        void openFor(double duration);
+        int getState();
+    
+    private:
+        int left_wing_state;
+        int right_wing_state;
+};
+
+#endif
\ No newline at end of file
diff --git a/CLOUDS/src/Wings.cpp b/CLOUDS/src/Wings.cpp
new file mode 100644
index 0000000..f491492
--- /dev/null
+++ b/CLOUDS/src/Wings.cpp
@@ -0,0 +1,76 @@
+/*
+    Wings.cpp
+    Controls all of the logic for the wings in autonomous and driver control
+*/
+
+#include "main.h"
+#include "Wings.h"
+
+using namespace globals;
+
+Wings::Wings() {
+    /*
+        The state of each wing needs to be manually tracked due to privitization errors.
+        We cannot read the values of the pistons directly, in order to work around this we need to track the state manually.
+    */
+    left_wing_state = 0;
+    right_wing_state = 0;
+    close(); // Force close the wings for safety
+};
+
+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_state = value; 
+};
+
+void Wings::toggleRight(int value) {
+    right_wing_piston.set_value(value);
+    right_wing_state = value;
+};
+
+/*
+    Opens the wings for a certain amount of time
+    openFor(double duration) -> duration in seconds, converted to milliseconds
+*/
+
+void Wings::openFor(double duration) {
+    open();
+    pros::delay(duration * 1000);
+    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;
+    }
+}
\ No newline at end of file