fixed TeleopPeriodic of LiftNode

msoe-vex · Feb 12, 2022 · 5d91aef · 5d91aef
1 parent 0327be5
commit 5d91aef
Showing 1 changed file with 30 additions and 12 deletions.
diff --git a/src/v5_hal/firmware/src/nodes/LiftNode.cpp b/src/v5_hal/firmware/src/nodes/LiftNode.cpp
@@ -51,21 +51,39 @@ int LiftNode::getPosition() {
 }
 
 void LiftNode::teleopPeriodic() {
-    if (m_top_limit_switch->getValue() || m_bottom_limit_switch->getValue()) {
-        m_left_motor->moveVoltage(0);
-        m_right_motor->moveVoltage(0);
-    } else {
-        if (m_controller->getController()->get_digital(pros::E_CONTROLLER_DIGITAL_R1)) {
-			m_left_motor->moveVoltage(MAX_MOTOR_VOLTAGE);
+    if(pros::E_CONTROLLER_DIGITAL_R1 && !pros::E_CONTROLLER_DIGITAL_R2) {
+        if(m_top_limit_switch->getValue()) {
+            m_left_motor->moveVoltage(0);
+		    m_right_motor->moveVoltage(0);
+        } else {
+            m_left_motor->moveVoltage(MAX_MOTOR_VOLTAGE);
             m_right_motor->moveVoltage(-1 * MAX_MOTOR_VOLTAGE);
-		} else if (m_controller->getController()->get_digital(pros::E_CONTROLLER_DIGITAL_R2)) {
-			m_left_motor->moveVoltage(-1 * MAX_MOTOR_VOLTAGE);
+        }
+    } else if(pros::E_CONTROLLER_DIGITAL_R1 && !pros::E_CONTROLLER_DIGITAL_R2) {
+        if (m_bottom_limit_switch) {
+            m_left_motor->moveVoltage(0);
+		    m_right_motor->moveVoltage(0);
+        } else {
+            m_left_motor->moveVoltage(-1*MAX_MOTOR_VOLTAGE);
             m_right_motor->moveVoltage(MAX_MOTOR_VOLTAGE);
-		} else {
-			m_left_motor->moveVoltage(0);
-			m_right_motor->moveVoltage(0);
-		}
+        }
+    } else {
+        m_left_motor->moveVoltage(0);
+		m_right_motor->moveVoltage(0);
+    }
+    /*
+    if (m_bottom_limit_switch->getValue() && pros::E_CONTROLLER_DIGITAL_R1) && !pros::E_CONTROLLER_DIGITAL_R2) {
+        m_left_motor->moveVoltage(MAX_MOTOR_VOLTAGE);
+        m_right_motor->moveVoltage(-1 * MAX_MOTOR_VOLTAGE);
+    } else if (m_bottom_limit_switch->getValue() && !pros::E_CONTROLLER_DIGITAL_R1) && pros::E_CONTROLLER_DIGITAL_R2){
+        m_left_motor->moveVoltage(0);
+		m_right_motor->moveVoltage(0);
+    } else if (m_top_limit_switch->getValue() && pros::E_CONTROLLER_DIGITAL_R1) && !pros::E_CONTROLLER_DIGITAL_R2) {
+        m_left_motor->moveVoltage(0);
+		m_right_motor->moveVoltage(0);
     }
+    
+    /*
 };
 
 void LiftNode::autonPeriodic() {