diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0188a2c1..1434cb85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,8 +13,11 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.littletonrobotics.urcl.URCL; + import static frc.robot.settings.Constants.DriveConstants.*; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.Pigeon2; import com.fasterxml.jackson.core.sym.Name; import com.pathplanner.lib.auto.AutoBuilder; @@ -77,9 +80,9 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.Angle; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; -import frc.robot.commands.ManualShoot; import frc.robot.commands.AngleShooter; import frc.robot.commands.IntakeCommand; import frc.robot.settings.IntakeDirection; @@ -138,7 +141,11 @@ public RobotContainer() { Preferences.initBoolean("Use Limelight", false); Preferences.initBoolean("Use 2 Limelights", false); Preferences.initDouble("wait # of seconds", 0); - + + // DataLogManager.start(); + // URCL.start(); + // SignalLogger.setPath("/media/sda1/ctre-logs/"); + // SignalLogger.start(); driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); @@ -204,7 +211,7 @@ private void indexInit() { indexer = new IndexerSubsystem(); } private void indexCommandInst() { - defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button); + defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button, driverController::getPOV); indexer.setDefaultCommand(defaulNoteHandlingCommand); } @@ -242,7 +249,7 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL), () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getL2Button)); - new Trigger(driverController::getOptionsButton).whileTrue(new InstantCommand(driveTrain::forceUpdateOdometryWithVision)); + new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false))); new Trigger(driverController::getSquareButton).onTrue(new SequentialCommandGroup( new CollectNote(driveTrain, limelight), new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) @@ -254,7 +261,7 @@ private void configureBindings() { SmartDashboard.putData("Manual Angle Shooter Up", new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.MAXIMUM_SHOOTER_ANGLE)); } if(indexerExists) { - new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer)); + new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer, driverController::getPOV)); } if(climberExists) { // new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop())); diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index a47b0040..6893b29e 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -30,6 +30,7 @@ public class IndexCommand extends Command { BooleanSupplier revUpSupplier; Boolean revUp; BooleanSupplier shootIfReadySupplier; + DoubleSupplier ampSupplier;; Boolean shootIfReady; // DoubleSupplier POVSupplier; BooleanSupplier humanPlayerSupplier; @@ -42,16 +43,18 @@ public class IndexCommand extends Command { double runsEmpty = 0; /** Creates a new IndexCommand. */ - public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier) { + public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, DoubleSupplier ampSupplier) { this.m_Indexer = m_IndexerSubsystem; this.shootIfReadySupplier = shootIfReadySupplier; this.revUpSupplier = revUpSupplier; this.shooter = shooter; this.intake = intake; this.drivetrain = drivetrain; + this.ampSupplier = ampSupplier; this.angleShooterSubsytem = angleShooterSubsystem; this.humanPlayerSupplier = humanPlaySupplier; SmartDashboard.putNumber("amp RPS", AMP_RPS); + SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED); SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_IndexerSubsystem, shooter, intake); @@ -106,7 +109,11 @@ public void execute() { indexer = true; } if (indexer) { - m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + if(ampSupplier.getAsDouble() == 90) { + m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED)); + } else { + m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + } } else { m_Indexer.off(); } diff --git a/src/main/java/frc/robot/commands/ManualShoot.java b/src/main/java/frc/robot/commands/ManualShoot.java index e28135c0..8c800f14 100644 --- a/src/main/java/frc/robot/commands/ManualShoot.java +++ b/src/main/java/frc/robot/commands/ManualShoot.java @@ -6,15 +6,21 @@ import frc.robot.settings.Constants.IndexerConstants; import frc.robot.settings.Constants.ShooterConstants; import frc.robot.subsystems.IndexerSubsystem; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; public class ManualShoot extends Command { private IndexerSubsystem indexer; + DoubleSupplier ampSupplier; /** Creates a new ManualShoot. */ - public ManualShoot(IndexerSubsystem indexer) { + public ManualShoot(IndexerSubsystem indexer, DoubleSupplier ampSupplier) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(indexer); this.indexer = indexer; + this.ampSupplier = ampSupplier; } // Called when the command is initially scheduled. @@ -24,7 +30,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + if(ampSupplier.getAsDouble() == 90 || ampSupplier.getAsDouble() == 45|| ampSupplier.getAsDouble() == 135) { + indexer.set(IndexerConstants.INDEXER_AMP_SPEED); + } else { + indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index d7c1028e..71eeb947 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -108,7 +108,7 @@ private DriveConstants() { /** * The diameter of the module's wheel in meters. */ - public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.097; + public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.098; /** * The overall drive reduction of the module. Multiplying motor rotations by @@ -272,7 +272,8 @@ public static final class ShooterConstants{ public static final double AUTO_AIM_ROBOT_kD = 0.00; public static final double SHOOTING_RPS = 90; - public static final double AMP_RPS = 6.5; + public static final double AMP_RPS = 7.5; + //the PID values used on the PID loop on the motor controller that control the position of the shooter angle public static final double ANGLE_SHOOTER_POWER_KP = 0.026; @@ -291,7 +292,7 @@ public static final class ShooterConstants{ public static final double DISTANCE_MULTIPLIER = 0.15; public static final double OFFSET_MULTIPLIER = 1; public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found - public static final double MAXIMUM_SHOOTER_ANGLE = 98;//still has to be found + public static final double MAXIMUM_SHOOTER_ANGLE = 104;//still has to be found public static final double HUMAN_PLAYER_ANGLE = 97;//still has to be found public static final double HUMAN_PLAYER_RPS = -10;//still has to be found @@ -327,9 +328,10 @@ public static final class ClimberConstants{ } public static final class IndexerConstants{ public static final int INDEXER_MOTOR = 11; - public static final double INDEXER_INTAKE_SPEED = -0.25;//should be 0.5 TODO change to positive + public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; + public static final double INDEXER_AMP_SPEED = 0.4; } public static final class IntakeConstants{ public static final int INTAKE_1_MOTOR = 20; @@ -418,7 +420,7 @@ public final class Field{ public static final double SPEAKER_Z = 2.08; //height of opening public static final double MAX_SHOOTING_DISTANCE = 2491; - public static final double AMPLIFIER_ANGLE = 97; + public static final double AMPLIFIER_ANGLE = 101; //angle at 60 for bounce techinque, didn't work } @@ -426,7 +428,9 @@ public final class Vision{ public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-aprill"; public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-aprilr"; public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural"; - + + public static final double APRILTAG_CLOSENESS = 0.5; + public static final double MAX_TAG_DISTANCE = 3.05; public static final Translation2d fieldCorner = new Translation2d(16.54, 8.02); diff --git a/src/main/java/frc/robot/settings/LimelightFiducialData.java b/src/main/java/frc/robot/settings/LimelightFiducialData.java index c035b431..f72a829d 100644 --- a/src/main/java/frc/robot/settings/LimelightFiducialData.java +++ b/src/main/java/frc/robot/settings/LimelightFiducialData.java @@ -35,7 +35,7 @@ public Pose2d getbotPose(){ * @return true if the most recent vision pose estimation is inside the field and is close enough to the provided pose. */ public boolean isPoseTrustworthy(Pose2d robotPose){ - Pose2d poseEstimate = this.getbotPose(); + Pose2d poseEstimate = this.botPoseBlue; if ((poseEstimate.getX()