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

@ -162,8 +162,6 @@ 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());
@ -226,18 +224,18 @@ public class RobotContainer {
operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
@ -423,30 +421,30 @@ public class RobotContainer {
//sensorTab.add("odometry", drivetrain::getPose);
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())
.withSize(1,1).withPosition(2,1);
.withSize(2,1).withPosition(3,1);
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())
.withSize(1,1).withPosition(4,1);
.withSize(2,1).withPosition(7,1);
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())
.withSize(1,1).withPosition(6,1);
.withSize(2,1).withPosition(11,1);
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())
.withSize(1,1).withPosition(2,2);
.withSize(2,1).withPosition(3,2);
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())
.withSize(1,1).withPosition(4,2);
.withSize(2,1).withPosition(7,2);
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())
.withSize(1,1).withPosition(6,2);
.withSize(2,1).withPosition(9,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,16 +19,10 @@ 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 = 17;
public static final double kL1Position = 14;
public static final double kL2Position = 11;
public static final double kL3Position = 27;
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 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 kCoralIntakePosition = Units.degreesToRadians(175.0+90);

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