diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9c5cb4..d2e43ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import java.util.Optional; import java.util.OptionalDouble; @@ -27,6 +28,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -89,6 +91,7 @@ public class RobotContainer { vp.visualPose() ); }); + vision.addPoseEstimateConsumer((vp) -> { if(resetOdometryToVisualPose) { drivetrain.resetOdometry(vp.visualPose()); @@ -96,7 +99,6 @@ public class RobotContainer { } }); - driver = new CommandXboxController(OIConstants.kDriverControllerPort); secondary = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -104,8 +106,9 @@ public class RobotContainer { shiftTimer.reset(); configureBindings(); - //testConfigureBindings(); configureShiftDisplay(); + //testConfigureBindings(); + if(AutoConstants.kAutoConfigOk) { autoChooser = AutoBuilder.buildAutoChooser(); @@ -285,7 +288,17 @@ public class RobotContainer { driver.leftBumper().whileTrue(intakeRoller.runOut()); driver.rightBumper().whileTrue(intakeRoller.runIn()); - driver.rightTrigger().whileTrue(spindexer.spinToShooter()); + driver.rightTrigger().whileTrue( + spindexer.spinToShooter().alongWith( + intakeRoller.runIn(), + intakePivot.jimmy(.5) + ) + ); + + driver.rightTrigger().onFalse( + intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5) + ); + driver.b().whileTrue(spindexer.spinToIntake()); /* driver.b().whileTrue( drivetrain.lockToYaw( @@ -309,13 +322,6 @@ public class RobotContainer { //30 degrees good for shooter far near outpost secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10))); //10 degrees good for shooting ~33in away from hub - - secondary.y().whileTrue( - intakePivot.jimmy(.2) - .andThen( - intakePivot.manualSpeed(() -> -0.5) - ) - ); hood.setDefaultCommand(hood.trackToAngle(() -> { Pose2d drivetrainPose = drivetrain.getPose(); @@ -340,6 +346,24 @@ public class RobotContainer { return 0; } })); + + new Trigger(() -> MathUtil.isNear( + shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), + shooter.getAverageActualSpeeds(), + 150)).onTrue( + new FunctionalCommand( + () -> {}, + () -> { + driver.setRumble(RumbleType.kBothRumble, .75); + secondary.setRumble(RumbleType.kBothRumble, .75); + }, + (b) -> { + driver.setRumble(RumbleType.kBothRumble, 0); + secondary.setRumble(RumbleType.kBothRumble, 0); + }, + () -> false + ).withTimeout(5) + ); } private void configureNamedCommands() { diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index b946737..5a1d846 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -1,5 +1,6 @@ package frc.robot.constants; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 42ce8d1..236c516 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -12,7 +12,9 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -68,6 +70,21 @@ public class Hood extends SubsystemBase { @Override public void periodic() { + SmartDashboard.putNumber( + "HoodTargetDegrees", + Math.toDegrees(currentTargetDegrees) + ); + + SmartDashboard.putNumber( + "HoodCurrentAngle", + Math.toDegrees(encoder.getPosition()) + ); + + SmartDashboard.putBoolean( + "HoodAtSetpoint", + controller.isAtSetpoint() + ); + Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent()); Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees)); Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition())); diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java index 5aa2c16..00aea31 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -89,8 +89,8 @@ public class IntakePivot extends SubsystemBase { */ public Command jimmy(double time){ return Commands.repeatingSequence( - manualSpeed(() -> -0.75).withTimeout(time) - .andThen(manualSpeed(() -> 0.75).withTimeout(time)) + manualSpeed(() -> -0.75).withTimeout(time), + manualSpeed(() -> 0.75).withTimeout(time) ); } @@ -102,8 +102,8 @@ public class IntakePivot extends SubsystemBase { */ public Command jimmy(DoubleSupplier time) { return Commands.repeatingSequence( - manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()) - .andThen(manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())) + manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()), + manualSpeed(() -> 0.75).withTimeout(time.getAsDouble()) ); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index ac9f1ee..0481d9e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ShooterConstants; @@ -63,6 +64,30 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { + SmartDashboard.putNumber( + "ShooterTargetRPM", + targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()); + + SmartDashboard.putNumber( + "ShooterLeftSideRPM", + leftEncoder.getVelocity() + ); + + SmartDashboard.putNumber( + "ShooterRightSideRPM", + rightEncoder.getVelocity() + ); + + SmartDashboard.putBoolean( + "ShooterLeftSideAtSetpoint", + leftClosedLoopController.isAtSetpoint() + ); + + SmartDashboard.putBoolean( + "ShooterRightSideAtSetpoint", + rightClosedLoopController.isAtSetpoint() + ); + Logger.recordOutput( "Shooter/TargetRPM", targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM() @@ -73,6 +98,8 @@ public class Shooter extends SubsystemBase { Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity()); + Logger.recordOutput("Shooter/LeftRollers/OutputVoltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage()); + Logger.recordOutput("Shooter/RightRollers/OutputVoltage", rightMotor.getAppliedOutput() * rightMotor.getBusVoltage()); // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance? Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());