diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index fda91b30..9844d677 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -131,7 +131,7 @@ private void configureDriverBindings() { .onTrue(new ArmToFeed()) // .whileTrue(new SwerveDriveDriveToNote(driver)) .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.INTAKING)) + .deadlineWith(new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING)) .andThen(new BuzzController(driver)) ); @@ -139,7 +139,7 @@ private void configureDriverBindings() { driver.getLeftTriggerButton() .onTrue(new ArmToFeed()) .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.INTAKING)) + .deadlineWith(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) .andThen(new BuzzController(driver)) ) .whileTrue(new SwerveDriveDriveRobotRelative(driver)); @@ -155,7 +155,7 @@ private void configureDriverBindings() { .whileTrue(new ConditionalCommand( new ArmWaitUntilAtTarget() .withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) - .andThen(new ShooterFeederDeacquire()), + .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))), new SwerveDriveDriveAlignedSpeaker(driver) .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) @@ -165,10 +165,7 @@ private void configureDriverBindings() { .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) ) ) - .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN) - .until(() -> swerve.isAlignedToSpeaker()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ), + .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) .onFalse(new ConditionalCommand( new ShooterFeederStop(), @@ -186,10 +183,7 @@ private void configureDriverBindings() { .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) ) ) - .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN) - .until(() -> swerve.isAlignedToLobFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), @@ -208,10 +202,7 @@ private void configureDriverBindings() { .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) ) ) - .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN) - .until(() -> swerve.isAlignedToLowFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), @@ -247,10 +238,7 @@ private void configureDriverBindings() { .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) ) ) - .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL) - .until(() -> swerve.isAlignedToManualLobFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), @@ -268,19 +256,15 @@ private void configureDriverBindings() { .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) ) ) - .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL) - .until(() -> swerve.isAlignedToManualLowFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // climbing - driver.getRightButton().onTrue(new ArmToPreClimb()); - driver.getBottomButton().onTrue(new ArmToClimbing()); + // human player attention button + driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); } private void configureOperatorBindings() { diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index b7414684..2fdd3849 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -39,15 +39,8 @@ public LEDDefaultMode() { private LEDInstruction getInstruction() { - switch (arm.getState()) { - case PRE_CLIMB: - return LEDInstructions.ARM_PRECLIMB; - case CLIMBING: - return LEDInstructions.ARM_POSTCLIMB; - case AMP: - return LEDInstructions.AMP_WITHOUT_ALIGN; - default: - break; + if (Arm.getInstance().getState() == Arm.State.AMP) { + return LEDInstructions.ARM_AT_AMP; } if (intake.hasNote() || shooter.hasNote()) { diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index e9d9f1c4..36448853 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -55,29 +55,25 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction INTAKING = new LEDPulseColor(SLColor.BLUE); - LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.RED); + LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow(); + LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); + LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD); LEDInstruction SPEAKER_ALIGN = GREEN; - LEDInstruction SPEAKER_MANUAL = new LEDPulseColor(SLColor.GREEN); + LEDInstruction SPEAKER_MANUAL = new LEDRainbow(); - LEDInstruction LOW_FERRY_ALIGN = LIME; - LEDInstruction LOW_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.LIME); + LEDInstruction LOW_FERRY_ALIGN = PURPLE; + LEDInstruction LOW_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.PURPLE); - LEDInstruction LOB_FERRY_ALIGN = AQUA; - LEDInstruction LOB_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.AQUA); + LEDInstruction LOB_FERRY_ALIGN = GREEN; + LEDInstruction LOB_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.GREEN); - LEDInstruction AMP_WITH_ALIGN = VIOLET; - LEDInstruction AMP_WITHOUT_ALIGN = new LEDPulseColor(SLColor.VIOLET); - - LEDInstruction IS_ALIGNED = RAINBOW; + LEDInstruction ARM_AT_AMP = YELLOW; + LEDInstruction AMP_SCORE = GREEN; LEDInstruction ATTENTION = new LED694(0.01, SLColor.BLUE); - LEDInstruction ARM_PRECLIMB = ORANGE; - LEDInstruction ARM_POSTCLIMB = new LEDPulseColor(SLColor.ORANGE); - - LEDInstruction CONTAINS_NOTE = GOLD; + LEDInstruction CONTAINS_NOTE = RED; // TO FUTURE USERS, DONT PUT LEDAlign and LEDAutonChooser (any disabled LEDInstructions) inside // the LEDInstructions interface diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index cfc003b7..1e388bbb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -39,20 +39,20 @@ public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) { /** Classes to store all of the values a motor needs */ public interface Arm { - CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25); - CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25); + CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25, false); + CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, false); } public interface Intake { - CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.25); - CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25); - CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25); + CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.25, true); + CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true); + CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true); } public interface Shooter { - CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5); - CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5); - CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25); + CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5, true); + CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5, true); + CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, true); } /* Configurations */ @@ -91,24 +91,27 @@ public static class CANSparkConfig { public final IdleMode IDLE_MODE; public final int CURRENT_LIMIT_AMPS; public final double OPEN_LOOP_RAMP_RATE; + public final boolean ENABLE_VOLTAGE_COMPENSATION; public CANSparkConfig( boolean inverted, IdleMode idleMode, int currentLimitAmps, - double openLoopRampRate) { + double openLoopRampRate, + boolean enableVoltageCompensation) { this.INVERTED = inverted; this.IDLE_MODE = idleMode; this.CURRENT_LIMIT_AMPS = currentLimitAmps; this.OPEN_LOOP_RAMP_RATE = openLoopRampRate; + this.ENABLE_VOLTAGE_COMPENSATION = enableVoltageCompensation; } - public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) { - this(inverted, idleMode, currentLimitAmps, 0.0); + public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps, boolean enableVoltageCompensation) { + this(inverted, idleMode, currentLimitAmps, 0.0, enableVoltageCompensation); } - public CANSparkConfig(boolean inverted, IdleMode idleMode) { - this(inverted, idleMode, 80); + public CANSparkConfig(boolean inverted, IdleMode idleMode, boolean enableVoltageCompensation) { + this(inverted, idleMode, 80, enableVoltageCompensation); } public void configure(CANSparkBase motor) { @@ -116,6 +119,9 @@ public void configure(CANSparkBase motor) { motor.setIdleMode(IDLE_MODE); motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS); motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE); + if (ENABLE_VOLTAGE_COMPENSATION) { + motor.enableVoltageCompensation(12); + } motor.burnFlash(); } @@ -124,6 +130,9 @@ public void configureAsFollower(CANSparkMax motor, CANSparkMax follows) { motor.setIdleMode(IDLE_MODE); motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS); motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE); + if (ENABLE_VOLTAGE_COMPENSATION) { + motor.enableVoltageCompensation(12); + } motor.follow(follows); motor.burnFlash(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 79c1e535..005c34a2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -71,9 +71,7 @@ public boolean atTarget() { if (state == State.FEED) { return atValidFeedAngle(); } - boolean atTarget = Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get(); - SmartDashboard.putBoolean("test/armAtTarget", atTarget); - return atTarget; + return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get(); } @Override @@ -184,7 +182,7 @@ else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTrigger } else { controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); - if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING) { + if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE.get()) { setVoltage(controller.getOutput() + 0.31); } else { @@ -211,6 +209,8 @@ else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTrigger SmartDashboard.putNumber("Arm/Right Duty Cycle", rightMotor.get()); SmartDashboard.putNumber("Arm/Arm Angle", getDegrees()); - SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin) + SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin)] + + SmartDashboard.putBoolean("Arm/At Target", atTarget()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 83235d23..b1b6b8a4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -171,10 +171,7 @@ public boolean isAlignedToSpeaker() { Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); // Rotate by 180 because the shooter is on the back of the robot Rotation2d targetAngle = speakerPose.minus(currentPose).getAngle().rotateBy(Rotation2d.fromDegrees(180)); - SmartDashboard.putNumber("test/angleError", getPose().getRotation().minus(targetAngle).getDegrees()); - boolean isAligned = Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); - SmartDashboard.putBoolean("test/isAligned", isAligned); - return isAligned; + return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } public boolean isAlignedToLowFerry() {