diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c5bd385..8e7676f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -67,7 +67,7 @@ public class Constants { /*INTAKE CONFIG */ public static final double FLOOR_INTAKE_SPEED = 1.00; - public static final double SOURCE_INTAKE_SPEED = -0.50; + public static final double SOURCE_INTAKE_SPEED = -0.30; public static final int INTAKE_BEAM_BREAK_ID = 0; diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java index 1f689c1..e1fdd56 100644 --- a/src/main/java/frc/robot/InputtedCoDriverControls.java +++ b/src/main/java/frc/robot/InputtedCoDriverControls.java @@ -1,6 +1,7 @@ package frc.robot; import edu.wpi.first.wpilibj.XboxController; +import frc.robot.auton.AutonAction; import frc.robot.auton.AutonMoveHoodToPosition.HoodPosition; import frc.robot.robotcomponents.Climber; import frc.robot.robotcomponents.Intake; @@ -30,13 +31,13 @@ public static void onEveryFrame() { Shooter.stopShooterMotors(); } - if (controller.getAButtonPressed()) { - Intake.startSourceIntake(); - } - // TODO add this: && !Shooter.isPositioningRing() when mechanics adds a breambreak sensor (never) - if (controller.getAButtonReleased()) { - Intake.stopSourceIntake(); - } + // if (controller.getAButtonPressed()) { + // Intake.startSourceIntake(); + // } + // // TODO add this: && !Shooter.isPositioningRing() when mechanics adds a breambreak sensor (never) + // if (controller.getAButtonReleased()) { + // Intake.stopSourceIntake(); + // } if (controller.getBackButton()) { Climber.manualExtendArms(); @@ -57,16 +58,15 @@ public static void onEveryFrame() { // // TODO add 3rd position if needed // } if (controller.getBButton()) { - Intake.startFloorIntake(); + Intake.startSourceIntake(); // TODO add this: && !Shooter.isPositioningRing() when mechanics adds a breambreak sensor (never) } else if (controller.getBButtonReleased()) { - Intake.stopFloorIntake(); + Intake.stopSourceIntake(); } if (controller.getPOV() == 0) { Intake.backtrack(); Shooter.backtrack(); } else if (controller.getPOV() == -1 && lastPOV != -1) { - Intake.stopFloorIntake(); Shooter.stopShooterMotors(); } lastPOV = controller.getPOV(); diff --git a/src/main/java/frc/robot/InputtedDriverControls.java b/src/main/java/frc/robot/InputtedDriverControls.java index d957ad0..9486651 100644 --- a/src/main/java/frc/robot/InputtedDriverControls.java +++ b/src/main/java/frc/robot/InputtedDriverControls.java @@ -31,9 +31,6 @@ public static void onEveryFrame() { if (controller.getLeftBumper()) { Intake.startFloorIntake(); } - if (controller.getLeftBumperReleased()) { - Intake.stopFloorIntake(); - } //This isn't probably helpful, but if the controllers somehow switch mid match, this box will turn red if (controller.getRightX() <= -0.99 && Math.abs(controller.getRightY()) < 0.01) { framesWithBadInput++; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c0e1524..23f945e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -76,7 +76,7 @@ public class Robot extends TimedRobot { Constants.RIGHT_CLIMB_ID, MotorController.Type.SparkMaxBrushed ); - MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushed); + // MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushed); // MotorController hoodAdjuster = MotorControllerFactory.create( // Constants.SHOOTER_HOOD_ADJUSTERER_ID, // MotorController.Type.SparkMaxBrushed @@ -206,8 +206,8 @@ public boolean isFinished() { .leftFlywheel(leftFlywheel) .rightFlywheel(rightFlywheel) .leftClimb(leftClimb) - .rightClimb(rightClimb) - .intake(intake); + .rightClimb(rightClimb); + // .intake(intake); // .hoodAdjuster(hoodAdjuster); // getGyroscope().reset(); System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES); diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java index b8d16ab..02c7034 100644 --- a/src/main/java/frc/robot/RobotComponents/Intake.java +++ b/src/main/java/frc/robot/RobotComponents/Intake.java @@ -24,7 +24,6 @@ public static void startFloorIntake() { // if (!Robot.getTeleopActionRunner().containsAction(AutonFloorIntake.class)) { // Robot.getTeleopActionRunner().addActionToRun(new AutonFloorIntake()); // } - Robot.motors.getIntake().set(Constants.FLOOR_INTAKE_SPEED); Robot.motors.getFeeder().set(Constants.FLOOR_INTAKE_SPEED); } @@ -35,9 +34,7 @@ public static void startSourceIntake() { } /* This will spin the motor backwards in an attempt to eject a stuck note*/ - public static void backtrack() { - Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT); - } + public static void backtrack() {} public static void stopSourceIntake() { // Robot.getTeleopActionRunner().removeActionsOfType(AutonSourceIntake.class); @@ -46,7 +43,6 @@ public static void stopSourceIntake() { } public static void stopFloorIntake() { - Robot.motors.getIntake().stopMotor(); Robot.motors.getFeeder().stopMotor(); // Robot.getTeleopActionRunner().removeActionsOfType(AutonFloorIntake.class); } diff --git a/src/main/java/frc/robot/RobotComponents/Shooter.java b/src/main/java/frc/robot/RobotComponents/Shooter.java index 1abfdb3..526c642 100644 --- a/src/main/java/frc/robot/RobotComponents/Shooter.java +++ b/src/main/java/frc/robot/RobotComponents/Shooter.java @@ -74,4 +74,9 @@ public static void moveHoodToShootingPosition() { public static void stopHoodMotors() { Robot.motors.getHoodAdjuster().stopMotor(); } + + public static void stopFeedMotors() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'stopFeedMotors'"); + } } diff --git a/src/main/java/frc/robot/auton/AutonFloorIntake.java b/src/main/java/frc/robot/auton/AutonFloorIntake.java index e5d9e5c..e4b474b 100644 --- a/src/main/java/frc/robot/auton/AutonFloorIntake.java +++ b/src/main/java/frc/robot/auton/AutonFloorIntake.java @@ -24,14 +24,12 @@ public boolean isDone() { @Override public void initiate() { - Robot.motors.getIntake().set(Constants.FLOOR_INTAKE_SPEED); Robot.motors.getFeeder().set(Constants.FEEDER_MOTOR_SPEED); Shooter.setCurrentlyPositioningRing(false); } @Override public void shutdown() { - Robot.motors.getIntake().stopMotor(); Robot.motors.getFeeder().stopMotor(); Shooter.setCurrentlyPositioningRing(false); }