diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 588dccf..c7e102a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static class ArmConstants { public static final double ARM_INTAKE_DEGREES = -40; public static final int LEFT_MOTOR_ID = 5; - public static final int RIGHT_MOTOR_ID = 19; + public static final int RIGHT_MOTOR_ID = 19; public static final int RIGHT_ENCODER_ID = 6; public static final boolean ARE_MOTORS_REVERSED = false; @@ -102,8 +102,13 @@ public static class ArmConstants { } public static class IntakeShooterConstants { - public static final boolean INTAKE_REVERSE = false; - public static final boolean FLYWHEEL_REVERSE = false; + public static final boolean INTAKE_REVERSE = false; + public static final boolean FLYWHEEL_REVERSE = false; + + public static final int FLYWHEEL_MOTOR_LEFT_ID = 1; + public static final int FLYWHEEL_MOTOR_RIGHT_ID = 2; + public static final int INTAKE_MOTOR_LEFT_ID = 3; + public static final int INTAKE_MOTOR_RIGHT_ID = 4; } public static class RobotMovementConstants { @@ -255,8 +260,9 @@ public static class SwerveDrivetrainConstants { } public static class AutoConstants { - // preffered distance to tag, specifically for autopositioning the robot to in front of the tag - public static final double PREFERRED_TAG_DISTANCE=2; + // preffered distance to tag, specifically for autopositioning the robot to in + // front of the tag + public static final double PREFERRED_TAG_DISTANCE = 2; } public static class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a034180..9ba5bee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,17 +1,21 @@ package frc.robot; import frc.robot.Constants.DriverConstants; +import frc.robot.Constants.IntakeShooterConstants; import frc.robot.Constants.LightConstants; import frc.robot.Constants.SwerveDrivetrainConstants; import frc.robot.Constants.SwerveModuleConstants; import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.AutoConstants; import frc.robot.commands.Autos; import frc.robot.commands.CancelCommands; import frc.robot.commands.ChassisRemoteControl; +import frc.robot.commands.FollowTag; import frc.robot.commands.AimAtTag; import frc.robot.commands.ArmRotateTo; import frc.robot.commands.SetLightstripColor; import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.IntakeShooter; import frc.robot.subsystems.LightStrip; import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.SwerveModule; @@ -87,10 +91,16 @@ public class RobotContainer { ArmConstants.RIGHT_ENCODER_ID, ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm(); + private final IntakeShooter intakeShooter = new IntakeShooter( + IntakeShooterConstants.FLYWHEEL_MOTOR_LEFT_ID, + IntakeShooterConstants.FLYWHEEL_MOTOR_RIGHT_ID, + IntakeShooterConstants.INTAKE_MOTOR_LEFT_ID, + IntakeShooterConstants.INTAKE_MOTOR_RIGHT_ID); + private final SwerveDrivetrain drivetrain = new SwerveDrivetrain( - gyro, - swerveModuleFL, swerveModuleFR, - swerveModuleBL, swerveModuleBR); + gyro, + swerveModuleFL, swerveModuleFR, + swerveModuleBL, swerveModuleBR); private final SendableChooser autoChooser = new SendableChooser(); @@ -122,6 +132,8 @@ public void setUpDriveController() { final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT); final HIDType genericHIDType = genericHID.getType(); + final Command cancelCommand = new CancelCommands(drivetrain, arm); + drivetrain.removeDefaultCommand(); if (genericHIDType == null) { @@ -166,12 +178,14 @@ public void setUpDriveController() { // xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates, // drivetrain)); - xbox.b().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel)); xbox.povDown().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel)); xbox.povUp().onTrue(Commands.runOnce(inputs::increaseSpeedLevel)); - xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative)); + + xbox.b().onTrue(cancelCommand); + xbox.a().whileTrue(new AimAtTag(drivetrain, vision, 1, inputs)); + xbox.x().whileTrue(new FollowTag(drivetrain, vision, 1, AutoConstants.PREFERRED_TAG_DISTANCE)); drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs)); } @@ -186,7 +200,13 @@ public void setUpOperatorController() { final Command armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES); final Command armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); - final Command cancelCommand = new CancelCommands(drivetrain, arm); + final Command intake = Commands.runOnce(intakeShooter::intake, intakeShooter); + final Command startFlyWheel = Commands.runOnce(intakeShooter::startFlyWheels, intakeShooter); + + final Command amplifyLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_BLUE); + final Command coopLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_RED); + + final Command cancelCommand = new CancelCommands(drivetrain, arm, intakeShooter); if (genericHIDType == null) { SmartDashboard.putString("Operator Ctrl", "No Connection"); @@ -199,15 +219,22 @@ public void setUpOperatorController() { joystick.button(5).onTrue(armToAmp); joystick.button(6).onTrue(armToSpeaker); - joystick.button(7).onTrue(new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_BLUE)); - joystick.button(8).onTrue(new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_RED)); + joystick.button(7).onTrue(amplifyLightSignal); + joystick.button(8).onTrue(coopLightSignal); } else { SmartDashboard.putString("Operator Ctrl", "GamePad"); final CommandXboxController xbox = new CommandXboxController(genericHID.getPort()); + xbox.leftTrigger().onTrue(armToSpeaker); + xbox.leftBumper().onTrue(startFlyWheel); + xbox.rightTrigger().onTrue(armToIntake); - xbox.leftTrigger(5).onTrue(armToSpeaker); - xbox.povDown().onTrue(armToAmp); + xbox.rightBumper().onTrue(intake); + + xbox.a().onTrue(armToAmp); + + xbox.povLeft().onTrue(amplifyLightSignal); + xbox.povRight().onTrue(coopLightSignal); xbox.b().onTrue(cancelCommand); } @@ -225,5 +252,4 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } -} -; \ No newline at end of file +}; \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java index fc77107..8ea882c 100644 --- a/src/main/java/frc/robot/commands/FollowTag.java +++ b/src/main/java/frc/robot/commands/FollowTag.java @@ -32,7 +32,7 @@ public class FollowTag extends Command { * @param loseTagAfterSeconds how long to wait before giving up on rediscover * tag, set to null to never finish */ - public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, int tagID) { + public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, Transform2d targetDistanceToTag) { this.drivetrain = drivetrain; this.vision = vision; @@ -53,12 +53,12 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetD addRequirements(drivetrain); } - public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Translation2d targetDistanceToTag, int tagID) { - this(drivetrain, vision, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID); + public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, Translation2d targetDistanceToTag) { + this(drivetrain, vision, tagID, new Transform2d(targetDistanceToTag, new Rotation2d())); } - public FollowTag(SwerveDrivetrain drivetrain, Vision vision, double targetDistanceToTag, int tagID) { - this(drivetrain, vision, new Transform2d(new Translation2d(targetDistanceToTag, 0), new Rotation2d()), tagID); + public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, double targetDistanceToTag) { + this(drivetrain, vision, tagID, new Transform2d(new Translation2d(targetDistanceToTag, 0), new Rotation2d())); } @Override @@ -84,6 +84,13 @@ public void execute() { rotationSpeed = -rotationController.calculate(rotation.getRadians()); } + double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED; + double maxSpeed = Math.max(Math.abs(forwardSpeed), Math.abs(leftSpeed)); + if (maxSpeed > speedLimit) { + forwardSpeed = (forwardSpeed / maxSpeed) * speedLimit; + leftSpeed = (leftSpeed / maxSpeed) * speedLimit; + } + drivetrain.setDesiredState(new ChassisSpeeds(forwardSpeed, leftSpeed, rotationSpeed)); drivetrain.updateSmartDashboard(); } diff --git a/src/main/java/frc/robot/subsystems/IntakeShooter.java b/src/main/java/frc/robot/subsystems/IntakeShooter.java index 2bbe692..494c50a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeShooter.java +++ b/src/main/java/frc/robot/subsystems/IntakeShooter.java @@ -43,29 +43,34 @@ public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake intake2.setInverted(IntakeShooterConstants.INTAKE_REVERSE); } + public void setFlyWheelSpeed(double speed) { + flywheel1.set(speed); + flywheel2.set(speed); + } + public void startFlyWheels() { - flywheel1.set(1); - flywheel2.set(1); + setFlyWheelSpeed(1); } public void stopFlywheels() { - flywheel1.stopMotor(); - flywheel2.stopMotor(); + setFlyWheelSpeed(0); + } + + public void setIntakeSpeed(double speed) { + intake1.set(speed); + intake2.set(speed); } public void intake() { - intake1.set(1); - intake1.set(1); + setIntakeSpeed(1); } public void intakeReverse() { - intake1.set(-1); - intake1.set(-1); + setIntakeSpeed(-1); } public void stopIntake() { - intake1.stopMotor(); - intake1.stopMotor(); + setIntakeSpeed(0); } }