Skip to content

Commit

Permalink
Merge branch 'main' into 496-trap-score
Browse files Browse the repository at this point in the history
  • Loading branch information
@Meh243
Meh243 committed Mar 28, 2024
2 parents 2c0ded6 + e0ab0f6 commit 5dae7aa
Show file tree
Hide file tree
Showing 8 changed files with 184 additions and 78 deletions.
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/paths/BL-F5.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,14 +38,14 @@
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"velocity": 0.0,
"rotation": 90.0,
"rotateFast": false
},
"reversed": false,
"folder": "Bottom side paths",
"previewStartingState": {
"rotation": 0,
"rotation": 90.0,
"velocity": 0
},
"useDefaultConstraints": true
Expand Down
34 changes: 20 additions & 14 deletions src/main/deploy/pathplanner/paths/PathFind-AMP.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,50 +3,56 @@
"waypoints": [
{
"anchor": {
"x": 2.245219347581553,
"y": 6.1716317718349725
"x": 1.845187834519813,
"y": 6.314132267904176
},
"prevControl": null,
"nextControl": {
"x": 2.245219347581553,
"y": 6.244929061286695
"x": 1.845187834519813,
"y": 6.702409801216
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.7614572372001893,
"y": 7.388366776733554
"x": 1.845187834519813,
"y": 7.69159304084374
},
"prevControl": {
"x": 1.7614572372001893,
"y": 7.327924283437372
"x": 1.8544325376939041,
"y": 7.69159304084374
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"rotationTargets": [
{
"waypointRelativePos": 0,
"rotationDegrees": 0.0,
"rotateFast": true
}
],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxVelocity": 2.0,
"maxAcceleration": 5.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": -90.0,
"rotateFast": false
"rotation": 90.0,
"rotateFast": true
},
"reversed": false,
"folder": "Align Paths",
"previewStartingState": {
"rotation": -88.92917554521306,
"rotation": 89.69844605013174,
"velocity": 0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}
29 changes: 27 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,8 +118,24 @@ public class PWM {
public static class ControllerConstants {
public static final int DriverControllerPort = 0;
public static final int CopilotControllerPort = 1;
public static final int ButtonBoxControllerPort = 2;

public static final double DEADBAND = 0.1;

public static class ButtonBox {
public static final int GRAY_TOPLEFT = 5;
public static final int PINK = 3;
public static final int GREEN = 4;
public static final int GRAY_TOPRIGHT = 6;
public static final int GRAY_BOTTOMLEFT = 2; // AXIS
public static final int PURPLE = 1;
public static final int RED = 2;
public static final int GRAY_BOTTOMRIGHT = 3; // AXIS
public static final int SHARE = 7;
public static final int OPTIONS = 8;
public static final int L3_SL = 9;
public static final int R3_SR = 10;
}
}

public static class AutonomousConstants {
Expand All @@ -129,8 +145,10 @@ public static class AutonomousConstants {
public static final double MAX_MODULE_VELOCITY = Units.feetToMeters(16); // f/s to m/s
public static final double DRIVE_BASE_RADIUS = Units.inchesToMeters(10.825);

public static final double CONTROL_LOOP_PERIOD = 0.01;

public static final double CONTROL_LOOP_PERIOD = 0.01; // constants

public static final PathConstraints PATHFINDING_CONSTRAINTS = new PathConstraints(2.0, 1.0,
3.0, 1.5);
public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get constants

public static final double BLUE_CHASE_BOUNDARY = 8.5; // The highest X value the robot can be at before ending.
Expand All @@ -153,6 +171,9 @@ public static class AutonomousConstants {
new Rotation2d(180));
public static final Pose2d SOURCE_SUB_C_STARTPOSE_RED = new Pose2d(new Translation2d(15.85, 4.35),
new Rotation2d(-120));

public static final Pose2d AMP_LOCATION_RED = new Pose2d(new Translation2d(14.4, 7.62),
new Rotation2d(90));
}

public static class TunerConstants {
Expand Down Expand Up @@ -639,6 +660,10 @@ public class CandConstants { // TODO get real
public static final double SOURCE_RPM = -300d; // TODO test
public static final double SOURCE_ANGLE = 90d; // TODO test

// Pass
public static final double NOTE_PASS_ANGLE = 55d;
public static final double NOTE_PASS_RPM = 5000d;

// TODO find time to shoot
public static final double TIME_TO_SHOOT = 1d; // Time in seconds it takes from indexer start to flywheel exit
}
Expand Down
28 changes: 22 additions & 6 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,13 @@
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.path.PathPlannerPath;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
Expand All @@ -18,6 +20,7 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.AutonomousConstants;
import frc.robot.Constants.CollisionConstants.CollisionType;
import frc.robot.Constants.ControllerConstants.ButtonBox;
import frc.robot.Constants.ControllerConstants;
import frc.robot.Constants.DrivetrainConstants;
import frc.robot.Constants.IndexerConstants;
Expand Down Expand Up @@ -47,6 +50,7 @@
import frc.robot.command.stopDrive;
import frc.robot.command.shoot.AmpShot;
import frc.robot.command.shoot.FlywheelIN;
import frc.robot.command.shoot.NotePass;
import frc.robot.command.shoot.PointBlankShot;
import frc.robot.command.shoot.SmartShoot;
import frc.robot.command.shoot.PivotUP;
Expand Down Expand Up @@ -84,6 +88,7 @@ public class RobotContainer extends LightningContainer {

public static XboxControllerFilter driver;
public static XboxControllerFilter coPilot;
public static Joystick buttonBox;

// Subsystems
public Swerve drivetrain;
Expand Down Expand Up @@ -124,6 +129,7 @@ protected void initializeSubsystems() {
coPilot = new XboxControllerFilter(ControllerConstants.CopilotControllerPort,
Constants.ControllerConstants.DEADBAND, -1, 1,
XboxControllerFilter.filterMode.SQUARED); // CoPilot controller
buttonBox = new Joystick(ControllerConstants.ButtonBoxControllerPort);

drivetrain = TunerConstants.getDrivetrain();
limelights = new Limelights();
Expand Down Expand Up @@ -175,7 +181,7 @@ protected void initializeNamedCommands() {
new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer)
.deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1)));
NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer));
NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE));
NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE, drivetrain));
NamedCommands.registerCommand("Collect-And-Go", new CollectAndGo(collector, flywheel, indexer));
NamedCommands.registerCommand("Point-At-Speaker",
new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver));
Expand Down Expand Up @@ -238,6 +244,11 @@ protected void configureButtonBindings() {
new Trigger(driver::getLeftBumper)
.whileTrue(new ComboPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver, limelights));

// new Trigger(driver::getBButton).whileTrue(new PathFindToAuton(
// PathPlannerPath.fromPathFile("PathFind-AMP"), drivetrain));
new Trigger(driver::getBButton).whileTrue(new PathFindToAuton(PathPlannerPath.fromPathFile("PathFind-AMP"), drivetrain));


// new Trigger(driver::getYButton)
// .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain));

Expand All @@ -257,13 +268,14 @@ protected void configureButtonBindings() {
.whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING)));
// new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel,
// pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING)));
new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot));
// new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot));
new Trigger(coPilot::getYButton).whileTrue(new NotePass(flywheel, pivot));
// new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel,
// pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING)));

new Trigger(coPilot::getAButton)
.whileTrue(new AmpShot(flywheel, pivot)
.deadlineWith(leds.enableState(LED_STATES.SHOOTING)));
new Trigger(coPilot::getAButton)
.whileTrue(new AmpShot(flywheel, pivot)
.deadlineWith(leds.enableState(LED_STATES.SHOOTING)));

/* BIAS */
new Trigger(() -> coPilot.getPOV() == 0)
Expand Down Expand Up @@ -345,6 +357,10 @@ protected void configureButtonBindings() {
new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED C", false))
.onTrue(new InstantCommand(
() -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_RED)));

/* Button Box */
new Trigger(() -> buttonBox.getRawButton(ButtonBox.PINK)).whileTrue(new PivotUP(pivot));
new Trigger(() -> buttonBox.getRawAxis(ButtonBox.GRAY_BOTTOMLEFT) == 1).whileTrue(new PivotUP(pivot));
}

@Override
Expand All @@ -368,7 +384,7 @@ protected void configureDefaultCommands() {
// coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING)));

if (!Constants.isMercury()) {
climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber));
climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getRightY(), () -> -coPilot.getLeftY(), climber));
}
}

Expand Down
6 changes: 2 additions & 4 deletions src/main/java/frc/robot/command/PathFindToAuton.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@

public class PathFindToAuton extends Command {

private AutoBuilder autoBuilder;
private Command pathFindCommand;
private PathPlannerPath autonPath;

Expand All @@ -25,15 +24,14 @@ public class PathFindToAuton extends Command {
*/
public PathFindToAuton(PathPlannerPath autonPath, Swerve drivetrain) {
this.autonPath = autonPath;
autoBuilder = new AutoBuilder();

addRequirements(drivetrain);
}

@Override
public void initialize() {
pathFindCommand = autoBuilder.pathfindThenFollowPath(
autonPath, AutonomousConstants.PATH_CONSTRAINTS);
pathFindCommand = AutoBuilder.pathfindThenFollowPath(
autonPath, AutonomousConstants.PATHFINDING_CONSTRAINTS);
pathFindCommand.schedule();
}

Expand Down
21 changes: 18 additions & 3 deletions src/main/java/frc/robot/command/PathToPose.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,10 @@

import com.pathplanner.lib.auto.AutoBuilder;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.Constants.AutonomousConstants;
Expand All @@ -13,29 +15,42 @@
public class PathToPose extends Command {

private XboxControllerFilter controller = RobotContainer.driver;
private Swerve drivetrain;
private PIDController headingController = new PIDController(0.1, 0, 0);
private Pose2d pathfindingPose;
Command pathFindCommand;

/*
* Pathfinds to a specific pose given
* @param pathfindingPose The pose to pathfind to
*/
public PathToPose(Pose2d pathfindingPose) {
public PathToPose(Pose2d pathfindingPose, Swerve drivetrain) {
this.pathfindingPose = pathfindingPose;
this.drivetrain = drivetrain;

addRequirements(drivetrain);
}

@Override
public void initialize() {
headingController.enableContinuousInput(0, 360);
headingController.setTolerance(2);
pathFindCommand = AutoBuilder.pathfindToPose(
pathfindingPose, AutonomousConstants.PATH_CONSTRAINTS);
pathFindCommand.schedule();
pathfindingPose, AutonomousConstants.PATHFINDING_CONSTRAINTS);
}

@Override
public void execute() {
if (controller.getYButton()) {
end(true);
}

if (!headingController.atSetpoint()){
double pidOutput = headingController.calculate(drivetrain.getPose().getRotation().getDegrees(), pathfindingPose.getRotation().getDegrees());
drivetrain.setField(0, 0, pidOutput);
} else if (headingController.atSetpoint() && !pathFindCommand.isScheduled()){
pathFindCommand.schedule();
}
}

@Override
Expand Down
Loading

0 comments on commit 5dae7aa

Please sign in to comment.