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.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
driver.start().whileTrue(drivetrain.resetToVision());
@ -224,18 +226,18 @@ public class RobotContainer {
operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
);
operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
);
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
);
@ -421,30 +423,30 @@ public class RobotContainer {
//sensorTab.add("odometry", drivetrain::getPose);
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())
.withSize(2,1).withPosition(3,1);
.withSize(1,1).withPosition(2,1);
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())
.withSize(2,1).withPosition(7,1);
.withSize(1,1).withPosition(4,1);
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())
.withSize(2,1).withPosition(11,1);
.withSize(1,1).withPosition(6,1);
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())
.withSize(2,1).withPosition(3,2);
.withSize(1,1).withPosition(2,2);
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())
.withSize(2,1).withPosition(7,2);
.withSize(1,1).withPosition(4,2);
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())
.withSize(2,1).withPosition(9,2);
.withSize(1,1).withPosition(6,2);
apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
.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 kPThetaController = 5;
public static final double kAlignPXYController = 2;
public static final double kAlignPThetaController = 5;
// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
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
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 kCoralIntakePosition = 0;
public static final double kL1Position = 14;
public static final double kL1Position = 17;
public static final double kL2Position = 11;
public static final double kL3Position = 27;
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))
);
pidHeading = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
pidHeading.setTolerance(Units.degreesToRadians(3));
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));
pidTranslationY = new PIDController(AutoConstants.kPXYController,0,0);
pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
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()){
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
}
}
}
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){
return startRun(() -> {
pidTranslationX.reset();
pidTranslationY.reset();
pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
},
() -> {
ChassisSpeeds controlEffort = driveController.calculate(
@ -459,7 +462,11 @@ public class Drivetrain extends SubsystemBase {
public Command resetToVision(){
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()));
}
});
}
}