Skip to content

Commit

Permalink
Merge pull request #143 from 2491-NoMythic/automaticMovingAmpShot
Browse files Browse the repository at this point in the history 
added feature to the amp shot that will line up the robot in front of the amp automatically
  • Loading branch information
@rflood07
rflood07 authored Nov 27, 2024
2 parents 89a06dc + 1ffb507 commit b5bda5a
Show file tree
Hide file tree
Showing 6 changed files with 166 additions and 50 deletions.
25 changes: 25 additions & 0 deletions src/main/deploy/pathplanner/autos/testAmpShotAuto.auto
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 2.786947863972064,
"y": 6.987301913535124
},
"rotation": 180.0
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "rotate path"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}
58 changes: 58 additions & 0 deletions src/main/deploy/pathplanner/paths/AmpShotSetup.path
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 1.8,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 1.8,
"y": 7.1
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.8,
"y": 7.7
},
"prevControl": {
"x": 1.8,
"y": 7.6000000000000005
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.4,
"rotationDegrees": -90.0,
"rotateFast": true
}
],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 2.3,
"maxAngularVelocity": 650.0,
"maxAngularAcceleration": 350.0
},
"goalEndState": {
"velocity": 0,
"rotation": -90.0,
"rotateFast": true
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": -90.0,
"velocity": 0
},
"useDefaultConstraints": true
}
107 changes: 70 additions & 37 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import static frc.robot.settings.Constants.ShooterConstants.PRAC_AMP_RPS;
import static frc.robot.settings.Constants.ShooterConstants.LONG_SHOOTING_RPS;

import java.util.Map;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;

Expand All @@ -17,6 +18,7 @@

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;
Expand Down Expand Up @@ -60,6 +62,7 @@
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
Expand All @@ -73,6 +76,8 @@
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.math.MathUtil;
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.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
Expand Down Expand Up @@ -120,6 +125,7 @@ public class RobotContainer {
private SendableChooser<Command> autoChooser;
private PowerDistribution PDP;

Alliance currentAlliance;
BooleanSupplier ZeroGyroSup;
BooleanSupplier AimWhileMovingSup;
BooleanSupplier ShootIfReadySup;
Expand Down Expand Up @@ -216,8 +222,10 @@ public RobotContainer() {
if(indexerExists) {indexInit();}
if(intakeExists && shooterExists && indexerExists && angleShooterExists) {indexCommandInst();}
Limelight.useDetectorLimelight(useDetectorLimelight);
configureDriveTrain();
configureBindings();
autoInit();
ampShotInit();
// Configure the trigger bindings
}
private void climbSpotChooserInit() {
Expand Down Expand Up @@ -273,7 +281,6 @@ private void indexCommandInst() {
}

private void autoInit() {
configureDriveTrain();
registerNamedCommands();
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
Expand Down Expand Up @@ -382,41 +389,7 @@ public boolean runsWhenDisabled() {
if(intakeExists&&indexerExists) {
new Trigger(intake::isNoteSeen).and(()->!intake.isNoteHeld()).and(()->DriverStation.isTeleop()).and(()->!AimWhileMovingSup.getAsBoolean()).onTrue(new IndexerNoteAlign(indexer, intake).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).withTimeout(5));
}
if(indexerExists&&shooterExists&&angleShooterExists) {
double indexerAmpSpeed;
double shooterAmpSpeed;
if(Preferences.getBoolean("CompBot", true)) {
shooterAmpSpeed = ShooterConstants.COMP_AMP_RPS;
indexerAmpSpeed = IndexerConstants.COMP_INDEXER_AMP_SPEED;
} else {
shooterAmpSpeed = ShooterConstants.PRAC_AMP_RPS;
indexerAmpSpeed = IndexerConstants.PRAC_INDEXER_AMP_SPEED;
}
SequentialCommandGroup scoreAmp = new SequentialCommandGroup(
// new InstantCommand(()->shooter.shootSameRPS(ShooterConstants.AMP_RPS), shooter),
new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter),
new MoveMeters(driveTrain, 0.06, 0.3, 0, 0),
// new WaitCommand(2),
new WaitUntil(()->(shooter.validShot() && driveTrain.getChassisSpeeds().vxMetersPerSecond == 0)),
new InstantCommand(()->indexer.magicRPS(indexerAmpSpeed), indexer),//45 worked but a bit too high
new WaitCommand(0.5),
new InstantCommand(()->intake.setNoteHeld(false))
);
SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed);
SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup(
new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter),
new MoveMeters(driveTrain, 0.015, 0.5, 0, 0),
new InstantCommand(driveTrain::pointWheelsInward, driveTrain),
new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem),
new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)),
new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)),
new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer),
new WaitCommand(0.5),
new InstantCommand(()->intake.setNoteHeld(false))
);
new Trigger(AmpAngleSup).whileTrue(orbitAmpShot);
SmartDashboard.putData("amp shot", scoreAmp);
}

SmartDashboard.putData("move 1 meter", new MoveMeters(driveTrain, 1, 0.2, 0.2, 0.2));
InstantCommand setOffsets = new InstantCommand(driveTrain::setEncoderOffsets) {
public boolean runsWhenDisabled() {
Expand Down Expand Up @@ -518,7 +491,7 @@ private void configureDriveTrain() {
driveTrain::getPose, // Pose2d supplier
driveTrain::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto
driveTrain::getChassisSpeeds,
driveTrain::driveWhileAimed,
driveTrain::drive,
new HolonomicPathFollowerConfig(
new PIDConstants(
k_XY_P,
Expand All @@ -539,6 +512,16 @@ private void configureDriveTrain() {
);
}

private Pose2d getAmpShotPose(Alliance currentAlliance) {
if(currentAlliance == null) { return new Pose2d(5,5, new Rotation2d(Math.toRadians(-90)));}
else {
if(currentAlliance == Alliance.Blue) {
return new Pose2d(1.83, 6.88, new Rotation2d(Math.toRadians(-90)));
} else {
return new Pose2d(14.75, 6.88, new Rotation2d(Math.toRadians(-90)));
}
}
}
private void registerNamedCommands() {
NamedCommands.registerCommand("awayFromPodium", new MoveMeters(driveTrain, 0.2, 1, 0, 0));
NamedCommands.registerCommand("stopDrivetrain", new InstantCommand(driveTrain::stop, driveTrain));
Expand Down Expand Up @@ -622,6 +605,53 @@ public void execute() {
}
NamedCommands.registerCommand("wait x seconds", new WaitCommand(Preferences.getDouble("wait # of seconds", 0)));
}
public void ampShotInit() {
if(indexerExists&&shooterExists&&angleShooterExists) {
double indexerAmpSpeed;
double shooterAmpSpeed;
if(Preferences.getBoolean("CompBot", true)) {
shooterAmpSpeed = ShooterConstants.COMP_AMP_RPS;
indexerAmpSpeed = IndexerConstants.COMP_INDEXER_AMP_SPEED;
} else {
shooterAmpSpeed = ShooterConstants.PRAC_AMP_RPS;
indexerAmpSpeed = IndexerConstants.PRAC_INDEXER_AMP_SPEED;
}
SequentialCommandGroup scoreAmp = new SequentialCommandGroup(
// new InstantCommand(()->shooter.shootSameRPS(ShooterConstants.AMP_RPS), shooter),
new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter),
new MoveMeters(driveTrain, 0.06, 0.3, 0, 0),
// new WaitCommand(2),
new WaitUntil(()->(shooter.validShot() && driveTrain.getChassisSpeeds().vxMetersPerSecond == 0)),
new InstantCommand(()->indexer.magicRPS(indexerAmpSpeed), indexer),//45 worked but a bit too high
new WaitCommand(0.5),
new InstantCommand(()->intake.setNoteHeld(false))
);
SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed);
/**
* the following code producs a command that will first pathfind to a pose right in front of the amplifier, then drive backwards into the amp, then run our shooters amp shot
* sequence as it was at the 2024 State competition.
*/
SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup(
new InstantCommand(()->shooter.setTargetVelocity(shooterAmpSpeed, shooterAmpSpeed, 50, 50), shooter),
new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem),
// new AutoBuilder().pathfindThenFollowPath(PathPlannerPath.fromPathFile("AmpShotSetup"), DEFAUL_PATH_CONSTRAINTS),
Commands.select(Map.of(
Alliance.Red, AutoBuilder.pathfindToPose(getAmpShotPose(Alliance.Red), DEFAULT_PATH_CONSTRAINTS),
Alliance.Blue, AutoBuilder.pathfindToPose(getAmpShotPose(Alliance.Blue), DEFAULT_PATH_CONSTRAINTS)
), ()->currentAlliance==null ? Alliance.Red : currentAlliance),
new MoveMeters(driveTrain, 0.9, -0.5, 0, 0),
new MoveMeters(driveTrain, 0.015, 0.5, 0, 0),
new InstantCommand(driveTrain::pointWheelsInward, driveTrain),
new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)),
new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)),
new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer),
new WaitCommand(0.5),
new InstantCommand(()->intake.setNoteHeld(false))
);
new Trigger(AmpAngleSup).whileTrue(orbitAmpShot);
SmartDashboard.putData("amp shot", scoreAmp);
}
}
public void teleopInit() {
if(climberExists) {
SequentialCommandGroup resetClimbers = new SequentialCommandGroup(
Expand Down Expand Up @@ -655,6 +685,9 @@ public void logPower(){
}
}
public void robotPeriodic() {
currentAlliance = DriverStation.getAlliance().get();
SmartDashboard.putBoolean("RobotPeriodicRan", true);
SmartDashboard.putString("AlliancePeriodic", currentAlliance == null? "null" : currentAlliance == Alliance.Red? "Red": "Blue" );
// logPower();
}
public void disabledPeriodic() {
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/goToPose/GoToAmp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

package frc.robot.commands.goToPose;

import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS;
import static frc.robot.settings.Constants.DriveConstants.DEFAULT_PATH_CONSTRAINTS;

import java.util.function.BooleanSupplier;

Expand All @@ -30,7 +30,7 @@ public GoToAmp(DrivetrainSubsystem drivetrain) {
public void initialize() {
// PathPlannerPath ampPath = PathPlannerPath.fromPathFile("goToAmp");
PathPlannerPath ampPath = PathPlannerPath.fromPathFile("ScoreAmp");
actualCommand = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAULT_PATH_CONSTRAINTS);
actualCommand.initialize();
}

Expand Down
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

package frc.robot.commands.goToPose;

import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS;
import static frc.robot.settings.Constants.DriveConstants.DEFAULT_PATH_CONSTRAINTS;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathPlannerPath;
Expand Down Expand Up @@ -40,38 +40,38 @@ public void initialize() {
switch (climbSpotChooser.getSelected()) {
case "L-Chain Left":
System.out.println("L-Chain Source!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "L-Chain Middle":
System.out.println("L-Chain Middle!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "L-Chain Right":
System.out.println("L-Chain Amp!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAULT_PATH_CONSTRAINTS);
case "Mid-Chain Left":
System.out.println("Mid-Chain Source!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "Mid-Chain Middle":
System.out.println("Mid-Chain Middle!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "Mid-Chain Right":
System.out.println("Mid-Chain Amp!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "R-Chain Left":
System.out.println("R-Chain Source!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "R-Chain Middle":
System.out.println("R-Chain Middle!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAULT_PATH_CONSTRAINTS);
break;
case "R-Chain Right":
System.out.println("R-Chain Amp!");
actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAUL_PATH_CONSTRAINTS);
actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAULT_PATH_CONSTRAINTS);
break;
default:
System.out.println("We broke!(Or went to the default command)");
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/settings/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,7 @@ public double getValue() {
public static final double k_BALANCE_TOLORANCE_DEGREES = 10.0;
public static final double k_BALANCE_TOLORANCE_DEG_PER_SEC = 1;

public static final PathConstraints DEFAUL_PATH_CONSTRAINTS = new PathConstraints(2, 1.5, Math.toRadians(360), Math.toRadians(360));
public static final PathConstraints DEFAULT_PATH_CONSTRAINTS = new PathConstraints(2, 1.5, Math.toRadians(360), Math.toRadians(360));

public static final double k_PICKUP_NOTE_ta_P = 1;
public static final double k_PICKUP_NOTE_ta_I = 0;
Expand Down

0 comments on commit b5bda5a

Please sign in to comment.