diff --git a/src/main/java/frc/robot/InputtedDriverControls.java b/src/main/java/frc/robot/InputtedDriverControls.java index 09a26bc..5de7f14 100644 --- a/src/main/java/frc/robot/InputtedDriverControls.java +++ b/src/main/java/frc/robot/InputtedDriverControls.java @@ -1,6 +1,7 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; +import frc.robot.RobotComponents.DriveTrain; public class InputtedDriverControls { @@ -11,6 +12,7 @@ public static void setDriverController(XboxController xboxController) { } public static void onEveryFrame() { + DriveTrain.driveTank(controller.getLeftY(), controller.getRightY()); if (controller.getBButtonPressed()) { stopAtFeedingDistance(); } @@ -20,11 +22,6 @@ public static void onEveryFrame() { if (controller.getXButtonPressed()) { stopAtShootingDistance(); } - driveTank(controller.getLeftY(), controller.getRightY()); - } - - private static void driveTank(double leftY, double rightY) { - throw new UnsupportedOperationException("Unimplemented method 'driveTank'"); } private static void stopAtShootingDistance() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8b030f8..1459330 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotComponents.DriveTrain; import frc.robot.Vision.AprilTagHighlighter; import frc.robot.auton.AutonAction; import frc.robot.auton.AutonRoutes; @@ -114,6 +115,7 @@ public void robotInit() { System.out.println("Is drive right parent inverted at end?? " + driveRightParent.getInverted()); InputtedCoDriverControls.setCoDriverController(coDriverController); + InputtedDriverControls.setDriverController(driverController); } /** @@ -207,17 +209,7 @@ public void teleopInit() {} @Override public void teleopPeriodic() { teleopActionRunner.onEveryFrame(); - // TODO: make slow mode - double leftY = driverController.getLeftY(); - double rightY = driverController.getRightY(); - QuickActions.stopDriveMotors(); - if (Math.abs(leftY) > Constants.CONTROLLER_DEADZONE) { - driveLeftParent.set(leftY); - } - if (Math.abs(rightY) > Constants.CONTROLLER_DEADZONE) { - driveRightParent.set(rightY); - } - // aprilTagHighlighter.doEveryTeleopFrame(driverController); + InputtedDriverControls.onEveryFrame(); InputtedCoDriverControls.onEveryFrame(); } diff --git a/src/main/java/frc/robot/RobotComponents/DriveTrain.java b/src/main/java/frc/robot/RobotComponents/DriveTrain.java new file mode 100644 index 0000000..565d739 --- /dev/null +++ b/src/main/java/frc/robot/RobotComponents/DriveTrain.java @@ -0,0 +1,18 @@ +package frc.robot.RobotComponents; + +import frc.robot.Constants; +import frc.robot.QuickActions; +import frc.robot.Robot; + +public class DriveTrain { + + public static void driveTank(double leftY, double rightY) { + QuickActions.stopDriveMotors(); + if (Math.abs(leftY) > Constants.CONTROLLER_DEADZONE) { + Robot.motors.getDriveLeftChild().set(leftY); + } + if (Math.abs(rightY) > Constants.CONTROLLER_DEADZONE) { + Robot.motors.getDriveRightParent().set(rightY); + } + } +}