diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b7..565b0808 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,6 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3cf12d98..99fccf14 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,11 +66,11 @@ public static class SwerveConstants { public static final Translation2d BR_POS = new Translation2d(MODULE_DIST, -MODULE_DIST); } - public static class RollerandPivotConstants { - public static final int lastmotorID = 0; - public static final int topmotorID = 2; - public static final int bottommotorID = 3; - public static final int motor1ID = 1; + public static class IntakeConstants { + public static final int INTEGRATION_MOTOR_ID = 19; + public static final int FRONT_MOTOR_ID = 17; + public static final int BACK_MOTOR_ID = 18; + public static final int PIVOT_MOTOR_ID = 16; public static final int sensorID = 0; public static final int extendedlimitswitchID = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29be766f..a9fa18dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,13 +37,14 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final IntakeRollersSubsystem intakeSubsystem = new IntakeRollersSubsystem(); private final BaseDriveController driveController = new DualJoystickDriveController(); private final BaseSwerveSubsystem baseSwerveSubsystem; private final IntakePivotSubsystem intakePivotSubsystem; - private final ShooterFlywheelSubsystem shooterSubsystem; - private final ShooterFeederSubsystem feederSubsystem; + private final IntakeRollersSubsystem intakeRollerSubsystem = new IntakeRollersSubsystem(); + + private final ShooterFlywheelSubsystem shooterFlywheelSubsystem; + private final ShooterFeederSubsystem shooterFeederSubsystem; private final ShooterPivotSubsystem shooterPivotSubsystem; private final ClimbSubsystem climbSubsystem; @@ -56,6 +57,7 @@ public class RobotContainer { private final JoystickButton rightBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value); private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value); + //private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); ChoreoTrajectory traj; @@ -68,10 +70,10 @@ public RobotContainer() { // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); baseSwerveSubsystem = new SwerveSubsystem(); intakePivotSubsystem = new IntakePivotSubsystem(); - feederSubsystem = new ShooterFeederSubsystem(); + shooterFeederSubsystem = new ShooterFeederSubsystem(); shooterPivotSubsystem = new ShooterPivotSubsystem(false); - shooterSubsystem = new ShooterFlywheelSubsystem(); + shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(); climbSubsystem = new ClimbSubsystem(); @@ -84,47 +86,21 @@ public RobotContainer() { private void configureBindings() { - /** - * Right trigger - run pivot forward at speed pulled - * Left trigger - run pivot backward at speed pulled - * A button - run flywheels while button held - * B button - run feed wheels while button held - */ - - // aButton.onTrue(new LoadNoteCommand(feederSubsystem)); - - // bButton.onTrue(new ShootNoteCommand(feederSubsystem)); - - // leftBumper.onTrue(new AutoAimCommand(pivotSubsystem)); - - // rightBumper.onTrue(new VerticalCommand(pivotSubsystem)); - - // xButton.onTrue(new ReadyShooterCommand(shooterSubsystem)); - - // yButton.onTrue(new StopShooterCommands(shooterSubsystem)); - - intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - intakePivotSubsystem.setPivotSpeed(-mechController.getLeftTriggerAxis()); - })); - - shooterSubsystem.setDefaultCommand(new InstantCommand(() -> { - intakePivotSubsystem.setPivotSpeed(mechController.getRightTriggerAxis()); - })); bButton.onTrue(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(.7); + shooterFeederSubsystem.setFeederMotorSpeed(.7); })); bButton.onFalse(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(0); + shooterFeederSubsystem.setFeederMotorSpeed(0); })); xButton.onTrue(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(-.7); + shooterFeederSubsystem.setFeederMotorSpeed(-.7); })); xButton.onFalse(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(0); + shooterFeederSubsystem.setFeederMotorSpeed(0); })); shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { @@ -132,6 +108,27 @@ private void configureBindings() { // pivotSubsystem.printCurrentAngle(); }, shooterPivotSubsystem)); + aButton.onTrue(new InstantCommand(() -> { + shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED); + })); + + aButton.onFalse(new InstantCommand(() -> { + shooterFlywheelSubsystem.setShooterMotorSpeed(0); + })); + + intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { + if(mechController.getPOV() == 90){ + intakeRollerSubsystem.setAllRollSpeed(.3, .3); + } else if (mechController.getPOV() == 270){ + intakeRollerSubsystem.setAllRollSpeed(-.3, -.3); + } else { + intakeRollerSubsystem.setAllRollSpeed(0, 0); + } + })); + + + + if(baseSwerveSubsystem instanceof SwerveSubsystem){ final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java index 02665c44..c480fb0e 100644 --- a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java @@ -1,6 +1,6 @@ package frc.robot.commands.intake.pivot; -import static frc.robot.Constants.RollerandPivotConstants.pivotcounterclockwise; +import static frc.robot.Constants.IntakeConstants.pivotcounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakePivotSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java index ac7154bf..1af95cec 100644 --- a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java @@ -2,7 +2,7 @@ package frc.robot.commands.intake.pivot; -import static frc.robot.Constants.RollerandPivotConstants.pivotclockwise; +import static frc.robot.Constants.IntakeConstants.pivotclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakePivotSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java index ac71e654..ee1233bb 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.intake.roller; -import static frc.robot.Constants.RollerandPivotConstants.pastsensortime; -import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; -import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; +import static frc.robot.Constants.IntakeConstants.pastsensortime; +import static frc.robot.Constants.IntakeConstants.rollersclockwise; +import static frc.robot.Constants.IntakeConstants.rollerscounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakeRollersSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java index fab8ca37..8b593209 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.intake.roller; -import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; -import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; +import static frc.robot.Constants.IntakeConstants.rollersclockwise; +import static frc.robot.Constants.IntakeConstants.rollerscounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakeRollersSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java index e5e7c805..e347cb41 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java @@ -1,9 +1,9 @@ package frc.robot.commands.intake.roller; -import static frc.robot.Constants.RollerandPivotConstants.pastsensortime; -import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; -import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; +import static frc.robot.Constants.IntakeConstants.pastsensortime; +import static frc.robot.Constants.IntakeConstants.rollersclockwise; +import static frc.robot.Constants.IntakeConstants.rollerscounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakeRollersSubsystem; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java index 5e09e34b..7499a9a3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.intake; -import static frc.robot.Constants.RollerandPivotConstants.*; +import static frc.robot.Constants.IntakeConstants.*; //import edu.wpi.first.wpilibj.Encoder; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -9,13 +9,13 @@ public class IntakePivotSubsystem extends SubsystemBase{ - private final com.ctre.phoenix.motorcontrol.can.TalonSRX motor1; + private final com.ctre.phoenix.motorcontrol.can.TalonSRX pivotMotor; // private final Encoder intakeencoder; private final DigitalInput extendedlimitswitch; private final DigitalInput retractedlimitswitch; public IntakePivotSubsystem(){ - motor1 = new TalonSRX(motor1ID); + pivotMotor = new TalonSRX(PIVOT_MOTOR_ID); //intakeencoder = new Encoder(1,2); extendedlimitswitch = new DigitalInput(extendedlimitswitchID); retractedlimitswitch = new DigitalInput(retractedlimitswitchID); @@ -30,7 +30,7 @@ public IntakePivotSubsystem(){ // } public void movePivot(double speed){ - motor1.set(TalonSRXControlMode.PercentOutput, speed); + pivotMotor.set(TalonSRXControlMode.PercentOutput, speed); } public boolean pivotisextended(){ @@ -52,7 +52,7 @@ public boolean pivotisretracted(){ } public void setPivotSpeed(double right){ - motor1.set(TalonSRXControlMode.PercentOutput, right); + pivotMotor.set(TalonSRXControlMode.PercentOutput, right); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java index 47a52dff..dd76d00d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java @@ -6,7 +6,7 @@ package frc.robot.subsystems.intake; -import static frc.robot.Constants.RollerandPivotConstants.*; +import static frc.robot.Constants.IntakeConstants.*; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -14,17 +14,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollersSubsystem extends SubsystemBase { - private final TalonSRX topmotor; - private final TalonSRX bottommotor; - private final TalonSRX lastmotor; + private final TalonSRX frontMotor; + private final TalonSRX backMotor; + private final TalonSRX integrationMotor; private final AnalogPotentiometer sensor; /** Creates a new ExampleSubsystem. */ public IntakeRollersSubsystem() { - lastmotor = new TalonSRX(lastmotorID); - topmotor = new TalonSRX(topmotorID); - bottommotor = new TalonSRX(bottommotorID); + integrationMotor = new TalonSRX(INTEGRATION_MOTOR_ID); + frontMotor = new TalonSRX(FRONT_MOTOR_ID); + backMotor = new TalonSRX(BACK_MOTOR_ID); sensor = new AnalogPotentiometer(sensorID); } @@ -38,26 +38,26 @@ public boolean sensorNow(){ } public void setRollSpeed(double top, double bottom){ - topmotor.set(TalonSRXControlMode.PercentOutput, top); - bottommotor.set(TalonSRXControlMode.PercentOutput, bottom); + frontMotor.set(TalonSRXControlMode.PercentOutput, top); + backMotor.set(TalonSRXControlMode.PercentOutput, bottom); } public void setAllRollSpeed(double topone, double bottomone){ - topmotor.set(TalonSRXControlMode.PercentOutput, topone); - lastmotor.set(TalonSRXControlMode.PercentOutput, topone); - bottommotor.set(TalonSRXControlMode.PercentOutput, bottomone); + frontMotor.set(TalonSRXControlMode.PercentOutput, topone); + integrationMotor.set(TalonSRXControlMode.PercentOutput, topone); + backMotor.set(TalonSRXControlMode.PercentOutput, bottomone); } public void setRollersOutwards(Boolean pressedA){ if(pressedA==true) - topmotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); - bottommotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); + frontMotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); + backMotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); } public void setRollersInwards(Boolean pressedB){ if(pressedB==true) - topmotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); - bottommotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); + frontMotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); + backMotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); }