Skip to content

Commit a5b5130

Browse files
Strflightmight09Strflightmight09
authored
Merge branch 'main' into voltage-based-swerve
2 parents ddebcde + 3c2fa08 commit a5b5130

28 files changed

+550
-207
lines changed

‎src/main/java/frc/trigon/robot/RobotContainer.java‎

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,24 +6,24 @@
66
package frc.trigon.robot;
77

88
import com.pathplanner.lib.auto.AutoBuilder;
9-
import edu.wpi.first.math.geometry.Pose3d;
109
import edu.wpi.first.wpilibj2.command.Command;
1110
import edu.wpi.first.wpilibj2.command.Commands;
1211
import edu.wpi.first.wpilibj2.command.InstantCommand;
1312
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
1413
import frc.trigon.robot.commands.CommandConstants;
1514
import frc.trigon.robot.commands.commandfactories.*;
15+
import frc.trigon.robot.constants.AutonomousConstants;
1616
import frc.trigon.robot.constants.CameraConstants;
1717
import frc.trigon.robot.constants.LEDConstants;
1818
import frc.trigon.robot.constants.OperatorConstants;
19-
import frc.trigon.robot.constants.PathPlannerConstants;
2019
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
2120
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
2221
import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
2322
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
2423
import frc.trigon.robot.subsystems.MotorSubsystem;
2524
import frc.trigon.robot.subsystems.armelevator.ArmElevator;
2625
import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
26+
import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
2727
import frc.trigon.robot.subsystems.climber.Climber;
2828
import frc.trigon.robot.subsystems.climber.ClimberCommands;
2929
import frc.trigon.robot.subsystems.climber.ClimberConstants;
@@ -92,21 +92,25 @@ private void bindDefaultCommands() {
9292
private void initializeGeneralSystems() {
9393
Flippable.init();
9494
LEDConstants.init();
95-
PathPlannerConstants.init();
95+
AutonomousConstants.init();
9696
}
9797

9898
private void bindControllerCommands() {
9999
OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
100-
OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
100+
// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
101101
OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
102102

103-
OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))));
104-
OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand());
103+
OperatorConstants.FLOOR_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getFloorAlgaeCollectionCommand());
104+
OperatorConstants.REEF_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getReefAlgaeCollectionCommand());
105105

106-
OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
106+
OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand());
107107
OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false));
108108
OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true));
109+
OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
109110

111+
OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())));
112+
OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE));
113+
OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection));
110114
OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand());
111115
}
112116

‎src/main/java/frc/trigon/robot/commands/CommandConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@
77
import edu.wpi.first.wpilibj2.command.InstantCommand;
88
import frc.trigon.robot.RobotContainer;
99
import frc.trigon.robot.commands.commandfactories.GeneralCommands;
10+
import frc.trigon.robot.constants.AutonomousConstants;
1011
import frc.trigon.robot.constants.OperatorConstants;
11-
import frc.trigon.robot.constants.PathPlannerConstants;
1212
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
1313
import lib.commands.CameraPositionCalculationCommand;
1414
import lib.commands.WheelRadiusCharacterizationCommand;
@@ -45,7 +45,7 @@ public class CommandConstants {
4545
() -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
4646
),
4747
WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand(
48-
PathPlannerConstants.ROBOT_CONFIG.moduleLocations,
48+
AutonomousConstants.ROBOT_CONFIG.moduleLocations,
4949
RobotContainer.SWERVE::getDriveWheelPositionsRadians,
5050
() -> RobotContainer.SWERVE.getHeading().getRadians(),
5151
(omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)),

‎src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
import edu.wpi.first.wpilibj2.command.*;
77
import frc.trigon.robot.RobotContainer;
88
import frc.trigon.robot.commands.commandfactories.GeneralCommands;
9-
import frc.trigon.robot.constants.PathPlannerConstants;
9+
import frc.trigon.robot.constants.AutonomousConstants;
1010
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
1111
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
1212
import lib.utilities.flippable.FlippableRotation2d;
@@ -44,24 +44,24 @@ public static Translation2d calculateDistanceFromTrackedGamePiece() {
4444
final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField);
4545
var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus());
4646
Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece);
47-
Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position);
47+
Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position);
4848
return distanceFromTrackedGamePiece;
4949
}
5050

5151
public static Command getDriveToGamePieceCommand(Supplier<Translation2d> distanceFromTrackedGamePiece) {
5252
return new SequentialCommandGroup(
53-
new InstantCommand(() -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)),
53+
new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)),
5454
SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
55-
() -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()),
56-
() -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()),
55+
() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()),
56+
() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()),
5757
CoralAutoDriveCommand::calculateTargetAngle
5858
)
5959
);
6060
}
6161

6262
public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) {
6363
return distanceFromTrackedGamePiece != null &&
64-
(distanceFromTrackedGamePiece.getNorm() > PathPlannerConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open
64+
(distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open
6565
}
6666

6767
public static FlippableRotation2d calculateTargetAngle() {

0 commit comments

Comments
 (0)