Skip to content

Commit fd4379e

Browse files
Strflightmight09Nummun14ShmayaR
authored
Autonomous (#19)
* added auton stuff temporarily. Need the integration to be done to finish * no more craashing * Orangize, add transporter, clean states * Wow * Small stuff * Ummmmm * Who did this??? * @ShmayaR, do the rest 🙏 * "Theoretically" fine * no erros Co-Authored-By: Nummun14 <[email protected]> * crashing is dumb Co-Authored-By: Nummun14 <[email protected]> * progress is made Co-Authored-By: Nummun14 <[email protected]> * Update AutonomousCommands.java Co-Authored-By: Nummun14 <[email protected]> * why it never see coral Co-Authored-By: Nummun14 <[email protected]> * Update CameraConstants.java Co-Authored-By: Nummun14 <[email protected]> * working on it Co-Authored-By: Nummun14 <[email protected]> * Update AutonomousCommands.java Co-Authored-By: Nummun14 <[email protected]> * WHY DOES IT EQUAL NULL Co-Authored-By: Nummun14 <[email protected]> * temp * 12 coral auto :) Co-Authored-By: Nummun14 <[email protected]> * no collect algae during auto Co-Authored-By: Nummun14 <[email protected]> * no hitting reef Co-Authored-By: Nummun14 <[email protected]> * made first coral different Co-Authored-By: Nummun14 <[email protected]> * stuff works better Co-Authored-By: Nummun14 <[email protected]> * to drives Co-Authored-By: Nummun14 <[email protected]> * added camera coordinates Co-Authored-By: Nummun14 <[email protected]> * oops Co-Authored-By: Nummun14 <[email protected]> --------- Co-authored-by: Nummun14 <[email protected]> Co-authored-by: Shm <[email protected]>
1 parent 3c2fa08 commit fd4379e

18 files changed

+436
-147
lines changed

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

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

88
import com.pathplanner.lib.auto.AutoBuilder;
9+
import com.pathplanner.lib.commands.PathPlannerAuto;
910
import edu.wpi.first.wpilibj2.command.Command;
1011
import edu.wpi.first.wpilibj2.command.Commands;
1112
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -40,11 +41,17 @@
4041
import lib.utilities.flippable.Flippable;
4142
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
4243

44+
import java.util.List;
45+
4346
public class RobotContainer {
44-
public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator();
47+
public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(
48+
CameraConstants.INTAKE_SIDE_REEF_TAG_CAMERA,
49+
CameraConstants.LEFT_REEF_TAG_CAMERA,
50+
CameraConstants.RIGHT_REEF_TAG_CAMERA
51+
);
4552
public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator(
4653
CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS,
47-
ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION,
54+
ObjectPoseEstimator.DistanceCalculationMethod.TRANSLATION,
4855
SimulatedGamePieceConstants.GamePieceType.CORAL,
4956
CameraConstants.OBJECT_DETECTION_CAMERA
5057
);
@@ -122,7 +129,44 @@ private void configureSysIDBindings(MotorSubsystem subsystem) {
122129
subsystem.setDefaultCommand(Commands.idle(subsystem));
123130
}
124131

132+
@SuppressWarnings("All")
125133
private void buildAutoChooser() {
126-
autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser());
134+
autoChooser = new LoggedDashboardChooser<>("AutoChooser");
135+
136+
final List<String> autoNames = AutoBuilder.getAllAutoNames();
137+
boolean hasDefault = false;
138+
139+
for (String autoName : autoNames) {
140+
final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName);
141+
final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true);
142+
143+
if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName)) {
144+
hasDefault = true;
145+
autoChooser.addDefaultOption(autoNonMirrored.getName(), autoNonMirrored);
146+
autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored);
147+
} else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) {
148+
hasDefault = true;
149+
autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored);
150+
autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored);
151+
} else {
152+
autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored);
153+
autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored);
154+
}
155+
}
156+
157+
if (!hasDefault)
158+
autoChooser.addDefaultOption("None", Commands.none());
159+
else
160+
autoChooser.addOption("None", Commands.none());
161+
162+
addCommandsToChooser(
163+
AutonomousCommands.getFloorAutonomousCommand(true),
164+
AutonomousCommands.getFloorAutonomousCommand(false)
165+
);
166+
}
167+
168+
private void addCommandsToChooser(Command... commands) {
169+
for (Command command : commands)
170+
autoChooser.addOption(command.getName(), command);
127171
}
128172
}

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

Lines changed: 0 additions & 77 deletions
This file was deleted.

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

Lines changed: 40 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
* A command class to assist in the intaking of a game piece.
2121
*/
2222
public class IntakeAssistCommand extends ParallelCommandGroup {
23-
private static final ProfiledPIDController
23+
static final ProfiledPIDController
2424
X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
2525
new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
2626
new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
@@ -42,7 +42,7 @@ public IntakeAssistCommand(AssistMode assistMode) {
4242
getTrackGamePieceCommand(),
4343
GeneralCommands.getContinuousConditionalCommand(
4444
GeneralCommands.getFieldRelativeDriveCommand(),
45-
getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece),
45+
getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR),
4646
() -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null
4747
)
4848
);
@@ -57,50 +57,62 @@ public IntakeAssistCommand(AssistMode assistMode) {
5757
* @param distanceFromTrackedGamePiece the position of the game piece relative to the robot
5858
* @return the command
5959
*/
60-
public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece) {
60+
public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece, double intakeAssistScalar) {
6161
return new SequentialCommandGroup(
6262
new InstantCommand(() -> resetPIDControllers(distanceFromTrackedGamePiece.get())),
6363
SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
64-
() -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getX(),
65-
() -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getY(),
66-
() -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get())
64+
() -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getX(),
65+
() -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getY(),
66+
() -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar)
6767
)
6868
);
6969
}
7070

7171
private Command getTrackGamePieceCommand() {
7272
return new RunCommand(() -> {
7373
if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null)
74-
distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece();
74+
distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece();
7575
});
7676
}
7777

78-
private static Translation2d calculateDistanceFromTrackedCGamePiece() {
78+
public static Translation2d calculateDistanceFromTrackedGamePiece() {
7979
final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
8080
final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot();
8181
if (trackedObjectPositionOnField == null)
8282
return null;
8383

8484
final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField);
85-
final Translation2d robotToTrackedGamepieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus());
86-
Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamepieceDistance);
87-
return robotToTrackedGamepieceDistance;
85+
final Translation2d robotToTrackedGamePieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus());
86+
Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamePieceDistance);
87+
return robotToTrackedGamePieceDistance;
8888
}
8989

90-
private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) {
90+
private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) {
91+
if (distanceFromTrackedGamePiece == null)
92+
return new Translation2d(0, 0);
9193
final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX());
9294
final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle().unaryMinus());
9395

94-
final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getX()));
95-
final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getY()));
96+
final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getX()));
97+
final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getY()));
9698

9799
if (assistMode.equals(AssistMode.ALTERNATE_ASSIST))
98100
return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput);
99-
return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput);
101+
return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput, intakeAssistScalar);
100102
}
101103

102-
private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) {
103-
return calculateThetaAssistPower(assistMode, distanceFromTrackedGamepiece.getAngle().plus(Rotation2d.k180deg).unaryMinus());
104+
private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) {
105+
if (distanceFromTrackedGamePiece == null || !assistMode.shouldAssistTheta)
106+
return 0;
107+
Rotation2d distanceAngle = distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus();
108+
Rotation2d robotAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation();
109+
Rotation2d diff = robotAngle.minus(distanceAngle);
110+
Logger.recordOutput("IntakeAssist/difference", diff);
111+
Logger.recordOutput("IntakeAssist/robotAngle", robotAngle);
112+
Logger.recordOutput("IntakeAssist/distanceAngle", distanceAngle);
113+
var pow = calculateThetaAssistPower(assistMode, distanceAngle, intakeAssistScalar);
114+
Logger.recordOutput("IntakeAssist/pow", pow);
115+
return pow;
104116
}
105117

106118
private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) {
@@ -115,25 +127,25 @@ private static Translation2d calculateAlternateAssistTranslationPower(Translatio
115127
return new Translation2d(xPower, yPower);
116128
}
117129

118-
private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) {
130+
private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput, double intakeAssistScalar) {
119131
final double
120132
xJoystickPower = joystickValue.getX(),
121133
yJoystickPower = joystickValue.getY();
122134
final double
123-
xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower,
124-
yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower;
135+
xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower, intakeAssistScalar) : xJoystickPower,
136+
yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower, intakeAssistScalar) : yJoystickPower;
125137

126138
return new Translation2d(xPower, yPower);
127139
}
128140

129-
private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset) {
141+
private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset, double intakeAssistScalar) {
130142
final double
131143
pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())),
132144
joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX();
133145

134146
if (assistMode.equals(AssistMode.ALTERNATE_ASSIST))
135147
return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue);
136-
return calculateNormalAssistPower(pidOutput, joystickValue);
148+
return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar);
137149
}
138150

139151
private static double clampToOutputRange(double value) {
@@ -144,15 +156,14 @@ private static double calculateAlternateAssistPower(double pidOutput, double pid
144156
return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower;
145157
}
146158

147-
private static double calculateNormalAssistPower(double pidOutput, double joystickPower) {
148-
final double intakeAssistScalar = OperatorConstants.INTAKE_ASSIST_SCALAR;
149-
return (pidOutput * intakeAssistScalar) + (joystickPower * (1 - intakeAssistScalar));
159+
private static double calculateNormalAssistPower(double pidOutput, double joystickPower, double scalar) {
160+
return (pidOutput * scalar) + (joystickPower * (1 - scalar));
150161
}
151162

152163
private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) {
153164
X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond);
154165
Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond);
155-
THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
166+
THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
156167
}
157168

158169
/**
@@ -164,15 +175,15 @@ public enum AssistMode {
164175
*/
165176
ALTERNATE_ASSIST(true, true, true),
166177
/**
167-
* Applies pid values to autonomously drive to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs
178+
* Applies pid values to autonomously drive to the game piece, scaled by the intake assist scalar in addition to the driver's inputs
168179
*/
169180
FULL_ASSIST(true, true, true),
170181
/**
171-
* Applies pid values to align to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs
182+
* Applies pid values to align to the game piece, scaled by the intake assist scalar in addition to the driver's inputs
172183
*/
173184
ALIGN_ASSIST(false, true, true),
174185
/**
175-
* Applies pid values to face the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs
186+
* Applies pid values to face the game piece, scaled by the intake assist scalar in addition to the driver's inputs
176187
*/
177188
ASSIST_ROTATION(false, false, true);
178189

0 commit comments

Comments
 (0)