diff --git a/src/main/java/frc/robot/commands/AdvancedSpinUpCommand.java b/src/main/java/frc/robot/commands/AdvancedSpinUpCommand.java new file mode 100644 index 0000000..2edc56f --- /dev/null +++ b/src/main/java/frc/robot/commands/AdvancedSpinUpCommand.java @@ -0,0 +1,40 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ShooterConstants; +import frc.robot.subsystems.ShooterSubsystem; + +public class AdvancedSpinUpCommand extends Command { + private final ShooterSubsystem shooterSubsystem; + + private final Pose2d robotPose; + + /** + * Command to spin up the shooter + * @param shooterSubsystem Instance of {@link ShooterSubsystem} + * @param robotPose Instance of {@link Pose2d} + */ + public AdvancedSpinUpCommand(ShooterSubsystem shooterSubsystem, Pose2d robotPose) { + this.shooterSubsystem = shooterSubsystem; + this.robotPose = robotPose; + + addRequirements(shooterSubsystem); + } + + @Override + public void execute() { + + if (robotPose.getX() < 9) { + shooterSubsystem.setSpeed(ShooterConstants.shooterRPM); + } else { + shooterSubsystem.setVoltage(0,0); + } + + } + + @Override + public void end(boolean interrupted) { + shooterSubsystem.setVoltage(0,0); + } +} diff --git a/src/main/java/frc/robot/commands/AmpCommand.java b/src/main/java/frc/robot/commands/AmpCommand.java index 11a5cd4..614ad2b 100644 --- a/src/main/java/frc/robot/commands/AmpCommand.java +++ b/src/main/java/frc/robot/commands/AmpCommand.java @@ -14,7 +14,7 @@ public class AmpCommand extends Command { /** - * Moves the indexer to the amp + * Moves the indexer to the amp and scores * @param indexerSubsystem The instance of {@link IndexerSubsystem} * @param controller An instance of {@link XboxController} */ diff --git a/src/main/java/frc/robot/commands/SpinUpCommand.java b/src/main/java/frc/robot/commands/BasicSpinUpCommand.java similarity index 85% rename from src/main/java/frc/robot/commands/SpinUpCommand.java rename to src/main/java/frc/robot/commands/BasicSpinUpCommand.java index 7dbeaf6..2a07dd7 100644 --- a/src/main/java/frc/robot/commands/SpinUpCommand.java +++ b/src/main/java/frc/robot/commands/BasicSpinUpCommand.java @@ -4,14 +4,14 @@ import frc.robot.constants.ShooterConstants; import frc.robot.subsystems.ShooterSubsystem; -public class SpinUpCommand extends Command { +public class BasicSpinUpCommand extends Command { private final ShooterSubsystem shooterSubsystem; /** * Command to spin up the shooter * @param shooterSubsystem Instance of {@link ShooterSubsystem} */ - public SpinUpCommand(ShooterSubsystem shooterSubsystem) { + public BasicSpinUpCommand(ShooterSubsystem shooterSubsystem) { this.shooterSubsystem = shooterSubsystem; addRequirements(shooterSubsystem); diff --git a/src/main/java/frc/robot/commands/RotateToNoteAndDriveCommand.java b/src/main/java/frc/robot/commands/RotateToNoteAndDriveCommand.java index 0b895bf..713c478 100644 --- a/src/main/java/frc/robot/commands/RotateToNoteAndDriveCommand.java +++ b/src/main/java/frc/robot/commands/RotateToNoteAndDriveCommand.java @@ -17,14 +17,22 @@ public class RotateToNoteAndDriveCommand extends Command { private final double visionRotation; private final ProfiledPIDController rotationPID = new ProfiledPIDController( - VisionConstants.rotateToNoteP, - VisionConstants.rotateToNoteI, - VisionConstants.rotateToNoteD, - VisionConstants.rotateToNoteConstraints + VisionConstants.rotateToP, + VisionConstants.rotateToI, + VisionConstants.rotateToD, + VisionConstants.rotateToConstraints ); private final XboxController controller; + /** + * Rotates the robot to face the speaker, while still allowing the driver to control forward and backward movement + * @param swerveSubsystem The instance of {@link SwerveSubsystem} + * @param controller The instance of {@link XboxController} + * @param forward The desired forward percentage of the robot + * @param sideways The desired sideways percentage of the robot + * @param radians The desired rotation speed of the robot + */ public RotateToNoteAndDriveCommand(SwerveSubsystem swerveSubsystem, XboxController controller, DoubleSupplier forward, DoubleSupplier sideways, DoubleSupplier radians) { this.swerveSubsystem = swerveSubsystem; this.forward = forward; @@ -50,6 +58,7 @@ public void execute() { true, true ); + controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); } else { swerveSubsystem.drive( forward.getAsDouble(), diff --git a/src/main/java/frc/robot/commands/RotateToSpeakerAndDriveCommand.java b/src/main/java/frc/robot/commands/RotateToSpeakerAndDriveCommand.java new file mode 100644 index 0000000..9cd5005 --- /dev/null +++ b/src/main/java/frc/robot/commands/RotateToSpeakerAndDriveCommand.java @@ -0,0 +1,84 @@ +package frc.robot.commands; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ShooterConstants; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.IndexerSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +import java.util.function.DoubleSupplier; + +public class RotateToSpeakerAndDriveCommand extends Command { + private final SwerveSubsystem swerveSubsystem; + private final DoubleSupplier forward; + private final DoubleSupplier sideways; + private final DoubleSupplier radians; + private final double odometryRotation; + + private final ProfiledPIDController rotationPID = new ProfiledPIDController( + VisionConstants.rotateToP, + VisionConstants.rotateToI, + VisionConstants.rotateToD, + VisionConstants.rotateToConstraints + ); + + /** + * Rotates the robot to face the speaker, while still allowing the driver to control forward and backward movement + * @param swerveSubsystem The instance of {@link SwerveSubsystem} + * @param forward The desired forward percentage of the robot + * @param sideways The desired sideways percentage of the robot + * @param radians The desired rotation speed of the robot + */ + public RotateToSpeakerAndDriveCommand(SwerveSubsystem swerveSubsystem, DoubleSupplier forward, DoubleSupplier sideways, DoubleSupplier radians) { + this.swerveSubsystem = swerveSubsystem; + this.forward = forward; + this.sideways = sideways; + this.radians = radians; + + //Rotation tolerance in radians + rotationPID.setTolerance(0.05); + + //Robot speed toward/away from the speaker (y-direction) + double robotYSpeed = swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond; + //Robot speed toward/away from the speaker (x-direction) + double robotXSpeed = swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond; + //Note speed in x-direction(forward, as opposed to up) + double noteSpeedX = robotXSpeed + ShooterConstants.shooterNoteSpeedX; + + //Getting speaker pose relative to alliance color + Pose2d speakerPose = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue)? ShooterConstants.speakerPoseBlue : ShooterConstants.speakerPoseRed; + //Current robot pose + Pose2d robotPose = swerveSubsystem.getPose(); + + //'Moving' speaker position based on robot speed + double speakerYTranslation = robotYSpeed * ((speakerPose.getX()-robotPose.getX()) / noteSpeedX); + + //Calculated angle to rotate to + double angle = Math.atan2((speakerPose.getY() + speakerYTranslation) - robotPose.getY(), speakerPose.getX()-robotPose.getX()); + + //Rotation PID Calculations + this.odometryRotation = rotationPID.calculate(robotPose.getRotation().getRadians(), angle); + + addRequirements(swerveSubsystem); + + } + + @Override + public void execute() { + + swerveSubsystem.drive( + forward.getAsDouble(), + sideways.getAsDouble(), + odometryRotation, + true, + true + ); + + } +} diff --git a/src/main/java/frc/robot/commands/SpeakerCommand.java b/src/main/java/frc/robot/commands/SpeakerCommand.java index 665b285..d353021 100644 --- a/src/main/java/frc/robot/commands/SpeakerCommand.java +++ b/src/main/java/frc/robot/commands/SpeakerCommand.java @@ -17,6 +17,8 @@ public class SpeakerCommand extends Command { /** * Command to shoot for the speaker once there is a note in the intake * @param indexerSubsystem Instance of {@link IndexerSubsystem} + * @param fieldRelativeSpeeds Instance of {@link ChassisSpeeds} + * @param swervePose Instance of {@link Pose2d} */ public SpeakerCommand(IndexerSubsystem indexerSubsystem, ChassisSpeeds fieldRelativeSpeeds, Pose2d swervePose) { @@ -28,11 +30,16 @@ public SpeakerCommand(IndexerSubsystem indexerSubsystem, ChassisSpeeds fieldRela @Override public void execute() { + + //Robot speed toward the speaker double robotXSpeed = fieldRelativeSpeeds.vxMetersPerSecond; + //Note speed in x-direction(forward, as opposed to up) double noteSpeedX = robotXSpeed + ShooterConstants.shooterNoteSpeedX; + //Initial calculated distance to shoot from the speaker double shootingDistanceFromSpeaker = noteSpeedX * ShooterConstants.timeToSpeakerHeightWithGravity; + //Robot distance to speaker double robotDistanceFromSpeaker = Math.sqrt( Math.pow( ( @@ -52,14 +59,17 @@ public void execute() { ) ); + //Difference between optimal location and current location double difference = robotDistanceFromSpeaker - shootingDistanceFromSpeaker; + //Accounting for indexer spinup time if (difference > 0) { shootingDistanceFromSpeaker += noteSpeedX * ShooterConstants.shootDelayTime; } else { shootingDistanceFromSpeaker -= noteSpeedX * ShooterConstants.shootDelayTime; } + //Removing negatives difference = Math.abs(robotDistanceFromSpeaker - shootingDistanceFromSpeaker); System.out.println("Robot Speed: " + robotXSpeed + @@ -69,6 +79,7 @@ public void execute() { "\n Difference: " + difference ); + //Execute shoot if (difference < 0.4){ indexerSubsystem.rotateAllWheelsPercent(1.0); System.out.println("Shooting!!!!!"); diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 184f279..7fb72e1 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -18,8 +18,8 @@ public class VisionConstants { public static final double alignRotationD = 0; public static final TrapezoidProfile.Constraints alignRotationConstraints = new TrapezoidProfile.Constraints(0.75, 0.5); - public static final double rotateToNoteP = 0.6; - public static final double rotateToNoteI = 0; - public static final double rotateToNoteD = 0; - public static final TrapezoidProfile.Constraints rotateToNoteConstraints = new TrapezoidProfile.Constraints(Math.PI, Math.PI/2); + public static final double rotateToP = 0.6; + public static final double rotateToI = 0; + public static final double rotateToD = 0; + public static final TrapezoidProfile.Constraints rotateToConstraints = new TrapezoidProfile.Constraints(Math.PI, Math.PI/2); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9de75ca..e0f1964 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.networktables.DoubleEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ShooterConstants; import frc.robot.utils.MathUtils; @@ -26,6 +27,7 @@ public class ShooterSubsystem extends SubsystemBase { private final DoubleEntry ShooterSpeedBottom = NetworkTableInstance.getDefault() .getTable("Shooter").getDoubleTopic("Bottom").getEntry(0.0); + public ShooterSubsystem(){ topMotor.setInverted(true); bottomMotor.setInverted(true); @@ -61,6 +63,13 @@ public void setVoltage(double v1, double v2) { bottomMotor.setVoltage(v2); } + /** + * Check if center limebreak is seeing something + * @return If the limebreak is seeing something + */ + + + @Override public void periodic() { ShooterSpeedBottom.set(getBottomMotorRPM());