Skip to content

Corner backup shot for defense #108

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

Closed
themehdev wants to merge 7 commits into from
Closed

Corner backup shot for defense #108

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
694c360
first pass
themehdev Apr 4, 2026
4703c51
fixed rotations
themehdev Apr 4, 2026
373b16a
finished corner shot
themehdev Apr 4, 2026
7a28ad8
revert sim
themehdev Apr 4, 2026
503b4ba
Fixed in sim and added distance check
themehdev Apr 7, 2026
14988cb
Merge branch 'main' into corner-backup-shot
themehdev Apr 8, 2026
aa676b7
fix constants
themehdev Apr 8, 2026
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
2 changes: 2 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 @@ -302,6 +302,8 @@ private void configureBindings() {
// While the right trigger is held, we will shoot into the hub or ferry. Binding A to the shaking of the shooter.
gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger())
.whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_));

gamepad_.a().whileTrue(RobotCommands.shootHubInCorner(shooter_, hopper_, intake_, drivebase_));

gamepad_.rightBumper()
.whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed()));
Expand Down
53 changes: 53 additions & 0 deletions src/main/java/frc/robot/commands/robot/RobotCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package frc.robot.commands.robot;

import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;

import java.util.Set;
Expand All @@ -9,6 +11,7 @@

import org.littletonrobotics.junction.Logger;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.units.measure.Distance;
Expand Down Expand Up @@ -53,6 +56,56 @@ public static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem i
);
}

public static Command shootHubInCorner(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive) {
return Commands.defer(() -> {
Supplier<Pose2d> cornerPose = () -> {
Pose2d rightTarget =
DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
? ShooterConstants.Positions.blueCornerDepot
: ShooterConstants.Positions.redCornerDepot;

Pose2d leftTarget =
DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
? ShooterConstants.Positions.blueCornerOutpost
: ShooterConstants.Positions.redCornerOutpost;

Pose2d target =
drive.getPose().getY() < ShooterConstants.Positions.centerLineY
? rightTarget
: leftTarget;
return target;
};

Supplier<Pose2d> immdPose = () -> {
Pose2d rightTarget =
DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
? ShooterConstants.Positions.blueCornerDepotWaypoint
: ShooterConstants.Positions.redCornerDepotWaypoint;

Pose2d leftTarget =
DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
? ShooterConstants.Positions.blueCornerOutpostWaypoint
: ShooterConstants.Positions.redCornerOutpostWaypoint;

Pose2d target =
drive.getPose().getY() < ShooterConstants.Positions.centerLineY
? rightTarget
: leftTarget;
return target;
};

Logger.recordOutput("Shooter/CornerDriveTarget", cornerPose.get());

if(Meters.of(drive.getPose().getTranslation().getDistance(cornerPose.get().getTranslation())).lt(ShooterConstants.Positions.distFromCornerToShootInCorner)) {
return DriveCommands.simplePathCommand(drive, cornerPose.get(), immdPose.get(), MetersPerSecond.of(3.0), MetersPerSecondPerSecond.of(3.0))
.andThen(shootHub(shooter, hopper, intake, drive));
}

return shootHub(shooter, hopper, intake, drive);

}, Set.of(shooter, hopper, intake, drive));
}

/**
* Shoots into the hub, without aiming. This should not be used in most situations,
* and only when you need additional flexibility like trying to squeeze
Expand Down
94 changes: 48 additions & 46 deletions src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
View file
Open in desktop
Copy link
Copy Markdown
Contributor Author

@themehdev themehdev Apr 7, 2026

Choose a reason for hiding this comment

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

There are a lot of weird github issues that make it seem like it is way more complicated than it is. I only added constants to the positions class, and got a rid of the ferryConstants classes that we were not using and only making things more confusing. I also indented some things correctly because otherwise I was unable to read which code was in what classes.

Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,10 @@
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Inches;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
Expand Down Expand Up @@ -49,69 +52,68 @@ public class ShooterConstants {
public static final Angle hoodMaxAngle = Degrees.of(75.0) ;
public static final Angle hoodMinAngle = Degrees.of(0.0) ;

public class FerryPositions{
public static final Translation2d blueOutpostTarget= new Translation2d(2.135, 1.639);
public static final Translation2d redOutpostTarget= new Translation2d(14.0,6.0);
public class PID {
// shooter
public static final double shooterkP = 0.5;
public static final double shooterkI = 0.0;
public static final double shooterkD = 0.0;
public static final double shooterkV = 0.132;
public static final double shooterkA = 0.0;
public static final double shooterkG = 0.0;
public static final double shooterkS = 0.0;
}

public class PID {
// shooter
public static final double shooterkP = 0.5;
public static final double shooterkI = 0.0;
public static final double shooterkD = 0.0;
public static final double shooterkV = 0.132;
public static final double shooterkA = 0.0;
public static final double shooterkG = 0.0;
public static final double shooterkS = 0.0;
}
public class MotionMagic {

public class MotionMagic {
// shooter
public static final double shooterkMaxVelocity = 1000.0;
public static final double shooterkMaxAcceleration = 3000.0;
public static final double shooterkJerk = 0.0;
}

// shooter
public static final double shooterkMaxVelocity = 1000.0;
public static final double shooterkMaxAcceleration = 3000.0;
public static final double shooterkJerk = 0.0;
}
public class SoftwareLimits {
public static final double hoodMaxAngle = 0.0;
public static final double hoodMinAngle = 0.0;
}

public class SoftwareLimits {
public static final double hoodMaxAngle = 0.0;
public static final double hoodMinAngle = 0.0;
}
public class Positions {
// Field Positions
public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536);
public static final Translation2d redHubPose = new Translation2d(11.938,4.034536);

public class Positions {
// Field Positions
public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536);
public static final Translation2d redHubPose = new Translation2d(11.938,4.034536);
public static final Distance allianceZone = Meters.of(4.5974);

public static final Distance allianceZone = Meters.of(4.5974);
public static final Distance blueAllianceWall = Meters.of(0);
public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength());

public static final Distance blueAllianceWall = Meters.of(0);
public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength());
public static final Translation2d blueTargetLeft = new Translation2d(1,6);
public static final Translation2d blueTargetRight = new Translation2d(1,2);

public static final Translation2d blueTargetLeft = new Translation2d(1,6);
public static final Translation2d blueTargetRight = new Translation2d(1,2);
public static final Translation2d redTargetLeft = new Translation2d(15,6);
public static final Translation2d redTargetRight = new Translation2d(15,2);

public static final Translation2d redTargetLeft = new Translation2d(15,6);
public static final Translation2d redTargetRight = new Translation2d(15,2);
public static final Translation2d blueBumpTargetLeft = new Translation2d(4, FieldConstants.layout.getFieldWidth() - 2.5);
public static final Translation2d blueBumpTargetRight = new Translation2d(4,2.5);

public static final Translation2d blueBumpTargetLeft = new Translation2d(4, FieldConstants.layout.getFieldWidth() - 2.5);
public static final Translation2d blueBumpTargetRight = new Translation2d(4,2.5);
public static final Translation2d redBumpTargetLeft = new Translation2d(FieldConstants.layout.getFieldLength() - 4, FieldConstants.layout.getFieldWidth() - 2.5);
public static final Translation2d redBumpTargetRight = new Translation2d(FieldConstants.layout.getFieldLength() - 4,2.5);

public static final Translation2d redBumpTargetLeft = new Translation2d(FieldConstants.layout.getFieldLength() - 4, FieldConstants.layout.getFieldWidth() - 2.5);
public static final Translation2d redBumpTargetRight = new Translation2d(FieldConstants.layout.getFieldLength() - 4,2.5);
public static final double centerLineY = 4.034536;

public static final double centerLineY = 4.034536;
public static final Pose2d blueCornerDepot = new Pose2d(Inches.of(23.09), Inches.of(23.429), new Rotation2d(Degrees.of(40.417)));
public static final Pose2d blueCornerDepotWaypoint = new Pose2d(Inches.of(34.511), Inches.of(33.155), new Rotation2d(Degrees.of(40.417)));
public static final Pose2d blueCornerOutpost = new Pose2d(Inches.of(23.09), Inches.of(294.261), new Rotation2d(Degrees.of(-40.417)));
public static final Pose2d blueCornerOutpostWaypoint = new Pose2d(Inches.of(34.511), Inches.of(284.535), new Rotation2d(Degrees.of(-40.417)));
public static final Pose2d redCornerDepot = new Pose2d(Inches.of(628.13), Inches.of(23.429), new Rotation2d(Degrees.of(180.0 - 40.417)));
public static final Pose2d redCornerDepotWaypoint = new Pose2d(Inches.of(616.709), Inches.of(33.155), new Rotation2d(Degrees.of(180.0 - 40.417)));
public static final Pose2d redCornerOutpost = new Pose2d(Inches.of(628.13), Inches.of(294.261), new Rotation2d(Degrees.of(180.0 + 40.417)));
public static final Pose2d redCornerOutpostWaypoint = new Pose2d(Inches.of(616.709), Inches.of(284.535), new Rotation2d(Degrees.of(180.0 + 40.417)));

public static final Distance distFromCornerToShootInCorner = Meters.of(2.0);
}

public class HoodPWMs {
public static final int hoodLeftPWMPort = 2;
public static final int hoodRightPWMPort = 0;
}

public class ferryPositions{
public static final Translation2d blueOutpostFerryTarget= new Translation2d(2.136,1.935); //ferry target for the blue outpost
public static final Translation2d redOutpostFerryTarget= new Translation2d(14.0,6.0); //ferry target for the red outpost
public static final Translation2d blueDepotFerryTarget= new Translation2d(2.0,6.0); //ferry target for the blue depot
public static final Translation2d redDepotFerryTarget= new Translation2d(14.0,2.0); //ferry target for the red depot
}
}
Loading