Skip to content
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

Cardinal (targets like processor) and Heading (reef orbit) #27

Merged
merged 1 commit into from
Feb 1, 2025
Merged

Cardinal (targets like processor) and Heading (reef orbit) #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
23 changes: 23 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -398,7 +398,30 @@ private void configureControllerBindings()
MetersPerSecond.of(1),
// The coral is ejected vertically downwards
Degrees.of(-75)))));

// Driver START Button: Auto Angle to closest Reef target TODO: FIX
m_driver
.povRight()
.whileTrue(
Commands.parallel(
RobotState.getInstance().setTargetCommand(RobotState.getInstance().chooseReefTarget()),
DriveCommands.joystickDriveAtAngle(
m_drive,
() -> -m_driver.getLeftY(),
() -> -m_driver.getLeftX(),
() -> RobotState.getInstance().getAngleToTarget(m_drive.getPose().getTranslation()))));

// Driver Right bumper: Cardinal direction to PROCESSOR
m_driver
.povUp()
.whileTrue(
Commands.parallel(
RobotState.getInstance().setTargetCommand(RobotState.TARGET.PROCESSOR),
DriveCommands.joystickDriveAtAngle(
m_drive,
() -> -m_driver.getLeftY(),
() -> -m_driver.getLeftX(),
() -> RobotState.getInstance().getAngleOfTarget().plus(Rotation2d.fromDegrees(180)))));
}

/**
Expand Down
62 changes: 59 additions & 3 deletions src/main/java/frc/robot/RobotState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,10 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import lombok.Getter;
import lombok.RequiredArgsConstructor;
import lombok.Setter;
Expand All @@ -28,11 +32,20 @@ public class RobotState {
@RequiredArgsConstructor
@Getter
public enum TARGET {
LEFT_CORAL_STATION(FieldConstants.CoralStation.leftCenterFace.getTranslation()),
RIGHT_CORAL_STATION(FieldConstants.CoralStation.rightCenterFace.getTranslation()),
REEF(FieldConstants.Reef.center);
LEFT_CORAL_STATION(FieldConstants.CoralStation.leftCenterFace.getTranslation(), FieldConstants.CoralStation.leftCenterFace.getRotation()),
RIGHT_CORAL_STATION(FieldConstants.CoralStation.rightCenterFace.getTranslation(), FieldConstants.CoralStation.rightCenterFace.getRotation()),
REEF(FieldConstants.Reef.center, null),
REEF_AB(FieldConstants.Reef.centerFaces[0].getTranslation(), FieldConstants.Reef.centerFaces[0].getRotation()),
REEF_CD(FieldConstants.Reef.centerFaces[5].getTranslation(), FieldConstants.Reef.centerFaces[5].getRotation()),
REEF_EF(FieldConstants.Reef.centerFaces[4].getTranslation(), FieldConstants.Reef.centerFaces[4].getRotation()),
REEF_GH(FieldConstants.Reef.centerFaces[3].getTranslation(), FieldConstants.Reef.centerFaces[3].getRotation()),
REEF_IJ(FieldConstants.Reef.centerFaces[2].getTranslation(), FieldConstants.Reef.centerFaces[2].getRotation()),
REEF_KL(FieldConstants.Reef.centerFaces[1].getTranslation(), FieldConstants.Reef.centerFaces[1].getRotation()),
PROCESSOR(FieldConstants.Processor.centerFace.getTranslation(), FieldConstants.Processor.centerFace.getRotation()),
BARGE(FieldConstants.Barge.middleCage, Rotation2d.fromDegrees(180));;

private final Translation2d targetTranslation;
private final Rotation2d targetRotation;
}

@Getter
Expand All @@ -53,6 +66,44 @@ public Rotation2d getAngleToTarget(Translation2d currentTranslation)
return target.targetTranslation.minus(currentTranslation).getAngle();
}

// For Cardinal targets, such as the Processor, Net, Barge, and Coral Substations
public Rotation2d getAngleOfTarget() {
if (target.targetRotation == null) {
return null;
}
// Return the angle to align to target
return target.targetRotation;
}

public TARGET chooseReefTarget() {

// Get the angle of the vector from the robot to the reef
double angle = getAngleToTarget(TARGET.REEF.getTargetTranslation()).getDegrees();

// Now get the angle of the nearest target
if (((angle >= -30) && (angle < 30))) {
return (TARGET.REEF_GH);
} else if ((angle >= 30) && (angle < 90)) {
return (TARGET.REEF_IJ);
} else if ((angle >= 90) && (angle < 150)) {
return (TARGET.REEF_KL);
} else if (((angle >= 150) && (angle <= 180)) || ((angle > -180) && (angle < -150))) {
return (TARGET.REEF_AB);
} else if ((angle >= -150) && (angle < -90)) {
return (TARGET.REEF_CD);
} else if ((angle >= -90) && (angle < -30)) {
return (TARGET.REEF_EF);
} else {
return (TARGET.REEF); // Catch all
}
}

public double getDistanceToTarget(Pose2d currentPose) {
return currentPose
.getTranslation()
.getDistance(target.targetTranslation);
}

private Translation2d getFuturePose()
{
// If magnitude of velocity is low enough, use current pose
Expand Down Expand Up @@ -91,4 +142,9 @@ private Translation2d getFuturePose()
feedArmAngleMap.put(6.0, -10.0);
feedArmAngleMap.put(7.0, -19.0);
}

public Command setTargetCommand(TARGET target) {
return Commands.startEnd(() -> setTarget(target), () -> setTarget(TARGET.REEF))
.withName("Set Robot Target: " + target.toString());
}
}