Skip to content

Separate PIDManager for Align To reef algae #342

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
junyu101 wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Separate PIDManager for Align To reef algae #342

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
edacf72
Fixed align bug
junyu101 Mar 29, 2025
0d05531
added separate pid for align to algae
junyu101 Mar 29, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 36 additions & 3 deletions ...ain/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.units.measure.Angle;
import xbot.common.command.BaseCommand;
import xbot.common.math.MathUtils;
import xbot.common.math.PIDDefaults;
import xbot.common.math.PIDManager;
import xbot.common.math.XYPair;
import xbot.common.subsystems.drive.control_logic.HeadingModule;

Expand All @@ -20,6 +23,8 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand {

final HeadingModule headingModule;

final PIDManager pidManager;

final AprilTagVisionSubsystemExtended vision;
final DriveSubsystem drive;
final PoseSubsystem pose;
Expand All @@ -31,13 +36,25 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand {
@Inject
public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory headingModuleFactory,
DriveSubsystem drive, PoseSubsystem pose,
AprilTagVisionSubsystemExtended vision, OperatorInterface oi) {
AprilTagVisionSubsystemExtended vision, OperatorInterface oi,
PIDManager.PIDManagerFactory pidManagerFactory) {
this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
this.vision = vision;
this.drive = drive;
this.pose = pose;
this.oi = oi;
this.addRequirements(drive);

this.pidManager = pidManagerFactory.create("AlgaePIDManager", new PIDDefaults(
1.08, // P
Copy link
Copy Markdown
Contributor

@aschokking aschokking Mar 30, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

are these values the same as the current drive pid as a default?

Copy link
Copy Markdown
Contributor

@Rongrrz Rongrrz Mar 30, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These are the default (same as positional pid), I don't think we have tuned it

0, // I
4.0, // D
0.0, // F
0.6, // Max output
-0.6, // Min output
0.05, // Error threshold
0.005, // Derivative threshold
0.2));// Time threshold):
}

@Override
Expand All @@ -60,12 +77,15 @@ public void execute() {

// Everything below here can and will be extracted because it is repeated code with
// DriveWithSnapToReefTagCommand
var centeringTranslation2d = drive.getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY));
var centeringTranslation2d = getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY));
XYPair centeringVector = new XYPair(centeringTranslation2d.getX(), centeringTranslation2d.getY());
centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees());

var fieldVectorTranslation2d = oi.driverGamepad.getLeftFieldOrientedVector();
XYPair fieldVectorXYPair = new XYPair(fieldVectorTranslation2d.getX(), fieldVectorTranslation2d.getY());
XYPair fieldVectorXYPair = new XYPair(
MathUtils.deadband(fieldVectorTranslation2d.getX(), 0.05),
MathUtils.deadband(fieldVectorTranslation2d.getY(), 0.05)
);
double railsSimilarityToDriver = fieldVectorXYPair.dotProduct(
new XYPair(
Math.cos(idealFinalHeading.in(Radians) + Math.PI),
Expand All @@ -84,4 +104,17 @@ public void execute() {
new XYPair()
);
}

public Translation2d getPowerForRelativePositionChange(Translation2d goalPosition) {
double goalMagnitude = goalPosition.getNorm();

if (Math.abs(goalMagnitude) < 0.0001) {
goalMagnitude = 0.0001;
}
Translation2d normalizedGoalVector = goalPosition.div(goalMagnitude);

double power = -pidManager.calculate(0, goalMagnitude);

return normalizedGoalVector.times(power);
}
}