diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5235589..e901dae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,9 @@ public class Constants { public static final int DRIVE_LEFT_CHILD_ID = 8; public static final int DRIVE_RIGHT_PARENT_ID = 11; public static final int DRIVE_RIGHT_CHILD_ID = 3; + public static final int SHOOTER_LEFT_FLYWHEEL_ID = 6; + public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 7; + public static final int SHOOTER_FEEDER_ID = 9; public static final int INTAKE_ID = 6101; /*CLIMB MOTOR IDS */ @@ -19,6 +22,7 @@ public class Constants { public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 1.0; /*The amount of seconds that the feeder should wait before turning off after shooting */ public static final double FEEDER_MOTOR_SHUTOFF_TIME = 0; + public static final double SHOOTING_VELOCITY_RPM = 4000; /*CLIMB CONFIG */ public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5; @@ -27,11 +31,16 @@ public class Constants { public static final double CLIMB_EXTENSION_SPEED = .25; /**the % of max power we give to the arm motors while retracting */ public static final double CLIMB_RETRACTION_SPEED = -.25; + + /*INTAKE CONFIG */ + public static final double INTAKE_SPEED = 0; + /*DRIVE TRAIN CONFIG */ public static final double CONTROLLER_DEADZONE = 0.06; /*CONTROLLER PORT IDs*/ public static final int DRIVER_CONTROLLER_ID = 0; + public static final int CODRIVER_CONTROLLER_ID = 1; /*START OF VISION CONSTANTS */ public static final int CAMERA_HEIGHT = 480; @@ -46,6 +55,7 @@ public class Constants { public static final double ROTATION_DEGREE_TOLERANCE = 3; public static final double ROTATION_DEGREE_PER_SECOND_TOLERANCE = 30; public static final Gains NORMAL_ROBOT_GAINS = new Gains(0.0005, 0.0, 0, 0, 0, 1.); + public static final double ROTATION_ERROR_DEGREES = 5.0; /*PHYSICAL ROBOT CONSTANTS */ public static final double WHEEL_CIRCUMFERENCE_INCHES = 8 * Math.PI; @@ -61,8 +71,7 @@ public class Constants { /** amount of time in ms to wait for confirmation */ public static final int TIMEOUT_MS = 30; public static final double ENCODER_ROTATION = 4096.0; - /* ROTATION PID CONSTANTS */ - public static final double ROTATION_ERROR_DEGREES = 5.; - public static final double SHOOTING_VELOCITY_RPM = 4000; + /*OTHER CONFIG */ + public static final double MOTOR_BACKTRACK_SPEED_PERCENT = -0.5; } diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java index 9cbe9a4..87a46fb 100644 --- a/src/main/java/frc/robot/InputtedCoDriverControls.java +++ b/src/main/java/frc/robot/InputtedCoDriverControls.java @@ -14,7 +14,7 @@ public static void setCoDriverController(XboxController xboxController) { } public static void onEveryFrame() { - if (controller.getAButtonPressed()) { + if (controller.getYButtonPressed()) { Shooter.startFeedMotors(); } if (controller.getXButton()) { @@ -23,7 +23,7 @@ public static void onEveryFrame() { Shooter.stopShooterMotors(); } - if (controller.getYButton()) { + if (controller.getLeftBumper()) { Intake.startFloorIntake(); } else { Intake.stopFloorIntake(); @@ -36,5 +36,13 @@ public static void onEveryFrame() { } else { Climber.stopClimbMotors(); } + + if (controller.getAButtonPressed()) { + Intake.backtrack(); + Shooter.backtrack(); + } else if (controller.getAButtonReleased()) { + Intake.stopFloorIntake(); + Shooter.stopShooterMotors(); + } } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 26c8802..a954f1e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,14 +39,23 @@ public class Robot extends TimedRobot { MotorController driveLeftChild = RobotConfigs.getLeftChild(); MotorController driveRightParent = RobotConfigs.getRightParent(); MotorController driveRightChild = RobotConfigs.getRightChild(); - MotorController leftFlywheel = MotorControllerFactory.create(6, MotorController.Type.SparkMax); - MotorController rightFlywheel = MotorControllerFactory.create(7, MotorController.Type.SparkMax); - MotorController feederMotor = MotorControllerFactory.create(9, MotorController.Type.SparkMax); + MotorController leftFlywheel = MotorControllerFactory.create( + Constants.SHOOTER_LEFT_FLYWHEEL_ID, + MotorController.Type.SparkMax + ); + MotorController rightFlywheel = MotorControllerFactory.create( + Constants.SHOOTER_RIGHT_FLYWHEEL_ID, + MotorController.Type.SparkMax + ); + MotorController feederMotor = MotorControllerFactory.create( + Constants.SHOOTER_FEEDER_ID, + MotorController.Type.SparkMax + ); MotorController leftClimb = MotorControllerFactory.create(Constants.LEFT_CLIMB_ID, MotorController.Type.Talon); MotorController rightClimb = MotorControllerFactory.create(Constants.RIGHT_CLIMB_ID, MotorController.Type.Talon); MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMax); XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID); - XboxController coDriverController = new XboxController(1); + XboxController coDriverController = new XboxController(Constants.CODRIVER_CONTROLLER_ID); static AHRS navX = new AHRS(SPI.Port.kMXP); SequentialActionRunner auton; static ParallelActionRunner teleopActionRunner = new ParallelActionRunner(); diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java index 096a44c..16c25bb 100644 --- a/src/main/java/frc/robot/RobotComponents/Intake.java +++ b/src/main/java/frc/robot/RobotComponents/Intake.java @@ -1,11 +1,17 @@ package frc.robot.robotcomponents; +import frc.robot.Constants; import frc.robot.Robot; public class Intake { public static void startFloorIntake() { - Robot.motors.getIntake().set(.5); + Robot.motors.getIntake().set(Constants.INTAKE_SPEED); + } + + /* 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 stopFloorIntake() { diff --git a/src/main/java/frc/robot/RobotComponents/Shooter.java b/src/main/java/frc/robot/RobotComponents/Shooter.java index 33dece2..9d8162b 100644 --- a/src/main/java/frc/robot/RobotComponents/Shooter.java +++ b/src/main/java/frc/robot/RobotComponents/Shooter.java @@ -15,6 +15,7 @@ public static void startShooterMotors() { public static void stopShooterMotors() { Robot.motors.getLeftFlywheel().stopMotor(); Robot.motors.getRightFlywheel().stopMotor(); + Robot.motors.getFeeder().stopMotor(); } public static void startFeedMotors() { @@ -22,6 +23,14 @@ public static void startFeedMotors() { Robot.getTeleopActionRunner().addActionToRun(new AutonShooterFeed()); } + /* This will spin the motor backwards in an attempt to eject a stuck note*/ + public static void backtrack() { + Robot.getTeleopActionRunner().removeActionsOfType(AutonShooterFeed.class, AutonShoot.class); + Robot.motors.getLeftFlywheel().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT); + Robot.motors.getRightFlywheel().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT); + Robot.motors.getFeeder().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT); + } + public static void autoShootIntoSpeaker() { Robot.getTeleopActionRunner().addActionToRun(new AutonShoot()); } diff --git a/src/main/java/frc/robot/auton/ParallelActionRunner.java b/src/main/java/frc/robot/auton/ParallelActionRunner.java index 2c912e4..35327a0 100644 --- a/src/main/java/frc/robot/auton/ParallelActionRunner.java +++ b/src/main/java/frc/robot/auton/ParallelActionRunner.java @@ -26,13 +26,14 @@ public void onEveryFrame() { // This method is important if you have code that could add two of the same action with opposite goals. // That would mean they would fight each other and never finish, which is bad news bears if you ask me. - public void removeActionsOfType(Class clazz) { + public void removeActionsOfType(Class... clazzez) { Iterator it = actions.iterator(); while (it.hasNext()) { AutonAction auction = it.next(); - if (auction.getClass().equals(clazz)) { - auction.shutdown(); - actions.remove(auction); + for (Class clazz : clazzez) { + if (auction.getClass().equals(clazz)) { + actions.remove(auction); + } } } }