Made ClawNode

msoe-vex · Feb 11, 2022 · 6c60717 · 6c60717
1 parent 3dbcd57
commit 6c60717
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 4 deletions.
diff --git a/src/v5_hal/firmware/include/nodes/ClawNode.h b/src/v5_hal/firmware/include/nodes/ClawNode.h
@@ -2,10 +2,11 @@
 
 #include "lib-rr/nodes/NodeManager.h"
 #include "lib-rr/nodes/subsystems/IClawNode.h"
+#include "pros/misc.h"
 
 class ClawNode : public IClawNode {
 public:
-    ClawNode(NodeManager* node_manager, std::string handle_name);
+    ClawNode(NodeManager* node_manager, std::string handle_name, ControllerNode* controller, ADIDIgitalOutNode* primary_claw, ADIDIgitalOutNode* back_claw);
 
     virtual void initialize();
 

diff --git a/src/v5_hal/firmware/lib b/src/v5_hal/firmware/lib
diff --git a/src/v5_hal/firmware/src/nodes/ClawNode.cpp b/src/v5_hal/firmware/src/nodes/ClawNode.cpp
@@ -1,6 +1,11 @@
 #include "nodes/ClawNode.h"
 
-ClawNode::ClawNode(NodeManager* node_manager, std::string handle_name): IClawNode(node_manager, handle_name) {
+ClawNode::ClawNode(NodeManager* node_manager, std::string handle_name, ControllerNode* controller, ADIDIgitalOutNode* primary_claw, ADIDIgitalOutNode* back_claw)
+: IClawNode(node_manager, handle_name), 
+m_controller(controller->getController()),
+m_primary_claw(primary_claw),
+m_back_claw(back_claw)
+ {
 
 }
 
@@ -17,7 +22,23 @@ void ClawNode::closeClaw() {
 }
 
 void ClawNode::teleopPeriodic() {
-
+    bool front_claw_open = false;
+	bool back_claw_open = false;
+	bool l1_current_state = m_controller->get_digital(pros::E_CONTROLLER_DIGITAL_L1);
+	bool l1_previous_state = false;
+	bool a_current_state = m_controller->get_digital(pros::E_CONTROLLER_DIGITAL_A);
+	bool a_previous_state = false;
+
+    if (l1_current_state == 1 && l1_previous_state == 0) {
+			front_claw_open->!front_claw_open;
+		}
+
+	if (a_current_state == 1 && a_previous_state == 0) {
+			back_claw_open = !back_claw_open;
+		}
+
+	l1_previous_state = l1_current_state;
+	a_previous_state = a_current_state;
 }
 
 void ClawNode::autonPeriodic() {