Merge branch '15in' of github.com:msoe-vex/rr-ros1-robot-code-templat…

…e into 15in
msoe-vex · Feb 12, 2022 · ffa5f4b · ffa5f4b
2 parents 0bf5ee6 + b5f5013
commit ffa5f4b
Show file tree

Hide file tree

Showing 2 changed files 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,20 +51,27 @@ 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 (m_controller->getController()->get_digital(pros::E_CONTROLLER_DIGITAL_R1) && 
+        !m_controller->getController()->get_digital(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 (m_controller->getController()->get_digital(pros::E_CONTROLLER_DIGITAL_R1) && 
+        !m_controller->getController()->get_digital(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);
     }
 };
 

diff --git a/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp b/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp
@@ -62,6 +62,17 @@ void TankDriveNode::resetEncoders() {
     m_motors.right_4_motor->resetEncoder();
 }
 
+TankDriveNode::FourMotorDriveEncoderVals TankDriveNode::getIntegratedEncoderVals() {
+    FourMotorDriveEncoderVals encoder_vals = {
+        m_motors.left_1_motor->getPosition(),
+        m_motors.left_2_motor->getPosition(),
+        m_motors.left_3_motor->getPosition(),
+        m_motors.left_4_motor->getPosition()
+    };
+
+    return encoder_vals;
+}
+
 void TankDriveNode::initialize() {
     resetEncoders();
 }