Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

tweaks to L1 and auto align good when PIs are on

Browse Source
This commit is contained in:
Team 2648 2025-03-28 23:33:24 -04:00
parent f6c2a82779
commit a19285cb0b
4 changed files with 36 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/main/java/frc/robot/RobotContainer.java
View File

@ -162,6 +162,8 @@ public class RobotContainer {
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false)); driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true)); driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
driver.start().whileTrue(drivetrain.resetToVision()); driver.start().whileTrue(drivetrain.resetToVision());
@ -224,18 +226,18 @@ public class RobotContainer {
operator.x().onTrue( operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.5, false)) .alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean()) .until(() -> driver.a().getAsBoolean())
); );
operator.b().onTrue( operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.5, false)) .alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean()) .until(() -> driver.a().getAsBoolean())
); );
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.5, false)) .alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean()) .until(() -> driver.a().getAsBoolean())
); );
@ -421,30 +423,30 @@ public class RobotContainer {
//sensorTab.add("odometry", drivetrain::getPose); //sensorTab.add("odometry", drivetrain::getPose);
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag()) apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
.withSize(2,1).withPosition(1,1); .withSize(1,1).withPosition(1,1);
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()) apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
.withSize(2,1).withPosition(3,1); .withSize(1,1).withPosition(2,1);
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()) apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
.withSize(2,1).withPosition(5,1); .withSize(1,1).withPosition(3,1);
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()) apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
.withSize(2,1).withPosition(7,1); .withSize(1,1).withPosition(4,1);
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()) apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
.withSize(2,1).withPosition(9,1); .withSize(1,1).withPosition(5,1);
apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected()) apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
.withSize(2,1).withPosition(11,1); .withSize(1,1).withPosition(6,1);
apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag()) apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
.withSize(2,1).withPosition(1,2); .withSize(1,1).withPosition(1,2);
apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX()) apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
.withSize(2,1).withPosition(3,2); .withSize(1,1).withPosition(2,2);
apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY()) apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
.withSize(2,1).withPosition(5,2); .withSize(1,1).withPosition(3,2);
apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist()) apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
.withSize(2,1).withPosition(7,2); .withSize(1,1).withPosition(4,2);
apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS()) apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
.withSize(2,1).withPosition(7,2); .withSize(1,1).withPosition(5,2);
apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected()) apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
.withSize(2,1).withPosition(9,2); .withSize(1,1).withPosition(6,2);
apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag()) apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
.withSize(2,1).withPosition(4,4); .withSize(2,1).withPosition(4,4);

6
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -19,10 +19,16 @@ public class AutoConstants {
public static final double kPXYController = 3.5; public static final double kPXYController = 3.5;
public static final double kPThetaController = 5; public static final double kPThetaController = 5;
public static final double kAlignPXYController = 2;
public static final double kAlignPThetaController = 5;
// Constraint for the motion profiled robot angle controller // Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
// TODO This is a constant being managed like a static rewriteable variable // TODO This is a constant being managed like a static rewriteable variable
public static RobotConfig kRobotConfig; public static RobotConfig kRobotConfig;

2
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -42,7 +42,7 @@ public class ElevatorConstants {
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kCoralIntakePosition = 0; public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 14; public static final double kL1Position = 17;
public static final double kL2Position = 11; public static final double kL2Position = 11;
public static final double kL3Position = 27; public static final double kL3Position = 27;
public static final double kL4Position = 50.5; public static final double kL4Position = 50.5;

17
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -115,13 +115,13 @@ public class Drivetrain extends SubsystemBase {
VecBuilder.fill(1, 1, Units.degreesToRadians(360)) VecBuilder.fill(1, 1, Units.degreesToRadians(360))
); );
pidHeading = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
pidHeading.setTolerance(Units.degreesToRadians(3)); pidHeading.setTolerance(Units.degreesToRadians(3));
pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180)); pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
pidTranslationX = new PIDController(AutoConstants.kPXYController,0,0); pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
pidTranslationX.setTolerance(Units.inchesToMeters(0.5)); pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
pidTranslationY = new PIDController(AutoConstants.kPXYController,0,0); pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
pidTranslationY.setTolerance(Units.inchesToMeters(0.5)); pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading); driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
@ -192,7 +192,7 @@ public class Drivetrain extends SubsystemBase {
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){ }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer); orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp()); m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
} }
} }
Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
@ -326,6 +326,9 @@ public class Drivetrain extends SubsystemBase {
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){ public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
return startRun(() -> { return startRun(() -> {
pidTranslationX.reset();
pidTranslationY.reset();
pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
}, },
() -> { () -> {
ChassisSpeeds controlEffort = driveController.calculate( ChassisSpeeds controlEffort = driveController.calculate(
@ -459,7 +462,11 @@ public class Drivetrain extends SubsystemBase {
public Command resetToVision(){ public Command resetToVision(){
return runOnce(() -> { return runOnce(() -> {
m_estimator.resetPose(orangePose2d); if(vision.getOrangeTagDetected()){
m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
}else if(vision.getBlackTagDetected()){
m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
}
}); });
} }
} }