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

Compare commits

base: Team_2648:a19285cb0bd9980d24054177558e8ae37611589f
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup
..
compare: Team_2648:dead4a72899f4aa2b470661e0750335522771b9c
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup

No commits in common. "a19285cb0bd9980d24054177558e8ae37611589f" and "dead4a72899f4aa2b470661e0750335522771b9c" have entirely different histories.
a19285cb0b ... dead4a7289

5 changed files with 22 additions and 37 deletions
Show Stats Expand all files Collapse all files

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

@ -163,8 +163,6 @@ public class RobotContainer {
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());
driver.rightBumper().whileTrue( driver.rightBumper().whileTrue(
@ -226,18 +224,18 @@ public class RobotContainer {
operator.x().onTrue( operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false)) .alongWith(manipulator.runManipulator(() -> 0.5, 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.85, false)) .alongWith(manipulator.runManipulator(() -> 0.5, 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.85, false)) .alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean()) .until(() -> driver.a().getAsBoolean())
); );
@ -423,30 +421,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(1,1).withPosition(1,1); .withSize(2,1).withPosition(1,1);
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()) apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
.withSize(1,1).withPosition(2,1); .withSize(2,1).withPosition(3,1);
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()) apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
.withSize(1,1).withPosition(3,1); .withSize(2,1).withPosition(5,1);
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()) apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
.withSize(1,1).withPosition(4,1); .withSize(2,1).withPosition(7,1);
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()) apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
.withSize(1,1).withPosition(5,1); .withSize(2,1).withPosition(9,1);
apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected()) apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
.withSize(1,1).withPosition(6,1); .withSize(2,1).withPosition(11,1);
apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag()) apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
.withSize(1,1).withPosition(1,2); .withSize(2,1).withPosition(1,2);
apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX()) apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
.withSize(1,1).withPosition(2,2); .withSize(2,1).withPosition(3,2);
apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY()) apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
.withSize(1,1).withPosition(3,2); .withSize(2,1).withPosition(5,2);
apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist()) apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
.withSize(1,1).withPosition(4,2); .withSize(2,1).withPosition(7,2);
apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS()) apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
.withSize(1,1).withPosition(5,2); .withSize(2,1).withPosition(7,2);
apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected()) apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
.withSize(1,1).withPosition(6,2); .withSize(2,1).withPosition(9,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,16 +19,10 @@ 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 = 17; public static final double kL1Position = 14;
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;

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

@ -34,7 +34,7 @@ public class ManipulatorPivotConstants {
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100 public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168 public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
public static final double kEncoderOffset = 0.04500000178813934; public static final double kEncoderOffset = 0.78-0.25;
public static final double kStartingPosition = Units.degreesToRadians(90); public static final double kStartingPosition = Units.degreesToRadians(90);
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90); public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);

15
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.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints); pidHeading = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
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.kAlignPXYController,0,0); pidTranslationX = new PIDController(AutoConstants.kPXYController,0,0);
pidTranslationX.setTolerance(Units.inchesToMeters(0.5)); pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0); pidTranslationY = new PIDController(AutoConstants.kPXYController,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);
@ -326,9 +326,6 @@ 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(
@ -462,11 +459,7 @@ public class Drivetrain extends SubsystemBase {
public Command resetToVision(){ public Command resetToVision(){
return runOnce(() -> { return runOnce(() -> {
if(vision.getOrangeTagDetected()){ m_estimator.resetPose(orangePose2d);
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()));
}
}); });
} }
} }