Skip to content

Commit

Permalink
one piece autos
Browse files Browse the repository at this point in the history
  • Loading branch information
IanShiii committed Sep 29, 2024
1 parent 70ed9f7 commit 29d5dc0
Show file tree
Hide file tree
Showing 9 changed files with 247 additions and 21 deletions.
52 changes: 52 additions & 0 deletions src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.7023114046228107,
"y": 6.739121939782341
},
"prevControl": null,
"nextControl": {
"x": 1.702311404622812,
"y": 6.739121939782341
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.9026396514331507,
"y": 6.9956624528633675
},
"prevControl": {
"x": 1.9026396514331507,
"y": 6.9956624528633675
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": "Misc",
"previewStartingState": {
"rotation": 60.94539590092292,
"velocity": 0
},
"useDefaultConstraints": true
}
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/paths/Blue Center to B.path
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
Expand Down
52 changes: 52 additions & 0 deletions src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.7319122330552369,
"y": 4.351321779566637
},
"prevControl": null,
"nextControl": {
"x": 1.5903362575955933,
"y": 2.8614134151345225
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.563724819757333,
"y": 1.4405736503780713
},
"prevControl": {
"x": 2.563724819757333,
"y": 1.4405736503780713
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": "Misc",
"previewStartingState": {
"rotation": -58.91249467652,
"velocity": 0
},
"useDefaultConstraints": true
}
52 changes: 52 additions & 0 deletions src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.7023114046228107,
"y": 6.739121939782341
},
"prevControl": null,
"nextControl": {
"x": 1.702311404622812,
"y": 6.739121939782341
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.9026396514331507,
"y": 6.9956624528633675
},
"prevControl": {
"x": 1.9026396514331507,
"y": 6.9956624528633675
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": "Misc",
"previewStartingState": {
"rotation": 60.94539590092292,
"velocity": 0.0
},
"useDefaultConstraints": true
}
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/paths/Red Center to B.path
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
Expand Down
52 changes: 52 additions & 0 deletions src/main/deploy/pathplanner/paths/Red Source Side Mobility.path
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.7319122330552369,
"y": 4.351321779566637
},
"prevControl": null,
"nextControl": {
"x": 1.5903362575955933,
"y": 2.8614134151345225
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.563724819757333,
"y": 1.4405736503780713
},
"prevControl": {
"x": 2.563724819757333,
"y": 1.4405736503780713
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": "Misc",
"previewStartingState": {
"rotation": -58.91249467652,
"velocity": 0.0
},
"useDefaultConstraints": true
}
41 changes: 23 additions & 18 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
package com.stuypulse.robot;

import java.util.concurrent.atomic.AtomicBoolean;

import com.ctre.phoenix6.Utils;
import com.pathplanner.lib.path.PathPlannerPath;
import com.stuypulse.robot.commands.BuzzController;
import com.stuypulse.robot.commands.arm.ArmToAmp;
import com.stuypulse.robot.commands.arm.ArmToClimbing;
Expand All @@ -19,7 +16,6 @@
import com.stuypulse.robot.commands.auton.DoNothingAuton;
import com.stuypulse.robot.commands.auton.Mobility;
import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF;
import com.stuypulse.robot.commands.auton.BCA.AltFourPieceBCA;
import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA;
import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF;
import com.stuypulse.robot.commands.auton.SideAutons.OnePieceAmpSide;
Expand All @@ -28,40 +24,32 @@
import com.stuypulse.robot.commands.intake.IntakeSetAcquire;
import com.stuypulse.robot.commands.intake.IntakeStop;
import com.stuypulse.robot.commands.leds.LEDDefaultMode;
import com.stuypulse.robot.commands.leds.LEDReset;
import com.stuypulse.robot.commands.leds.LEDSet;
import com.stuypulse.robot.commands.shooter.ShooterFeederAcquire;
import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire;
import com.stuypulse.robot.commands.shooter.ShooterFeederShoot;
import com.stuypulse.robot.commands.shooter.ShooterFeederStop;
import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget;
import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp;
import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker;
import com.stuypulse.robot.commands.vision.VisionChangeWhiteList;
import com.stuypulse.robot.commands.vision.VisionReloadWhiteList;
import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative;
import com.stuypulse.robot.constants.LEDInstructions;
import com.stuypulse.robot.constants.Ports;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.stuylib.input.Gamepad;
import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.robot.subsystems.intake.Intake;
import com.stuypulse.robot.subsystems.leds.LEDController;
import com.stuypulse.robot.subsystems.shooter.Shooter;
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
import com.stuypulse.robot.subsystems.swerve.Telemetry;
import com.stuypulse.robot.subsystems.vision.AprilTagVision;
import com.stuypulse.robot.util.PathUtil.AutonConfig;
import com.stuypulse.robot.util.ShooterLobFerryInterpolation;
import com.stuypulse.robot.util.ShooterSpeeds;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.robot.subsystems.intake.Intake;
import com.stuypulse.robot.subsystems.leds.LEDController;
import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction;
import com.stuypulse.robot.subsystems.leds.instructions.LEDPulseColor;
import com.stuypulse.robot.subsystems.leds.instructions.LEDRainbow;
import com.stuypulse.stuylib.input.Gamepad;
import com.stuypulse.stuylib.input.gamepads.AutoGamepad;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand Down Expand Up @@ -306,6 +294,23 @@ public void configureAutons() {
AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new,
"Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F");

// AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new,
// "Blue Amp Side Mobility");
// AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new,
// "Red Amp Side Mobility");

AutonConfig SOURCE_SIDE_ONE_PIECE_BLUE = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new,
"Blue Source Side Mobility");

AutonConfig SOURCE_SIDE_ONE_PIECE_RED = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new,
"Red Source Side Mobility");

// AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser);
// AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser);

SOURCE_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser);
SOURCE_SIDE_ONE_PIECE_RED.registerRed(autonChooser);

MOBILITY_BLUE.registerBlue(autonChooser);
MOBILITY_RED.registerRed(autonChooser);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,14 @@
import com.pathplanner.lib.path.PathPlannerPath;
import com.stuypulse.robot.commands.arm.ArmToFeed;
import com.stuypulse.robot.commands.auton.ShootRoutine;
import com.stuypulse.robot.commands.intake.IntakeSetAcquire;
import com.stuypulse.robot.subsystems.intake.Intake;
import com.stuypulse.robot.subsystems.shooter.Shooter;
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;

public class OnePieceAmpSide extends SequentialCommandGroup {

Expand All @@ -15,8 +20,13 @@ public OnePieceAmpSide(PathPlannerPath... paths) {
ShootRoutine.fromSubwoofer(),
new ArmToFeed(),

new WaitCommand(10),

new IntakeSetAcquire(),
// Mobility
SwerveDrive.getInstance().followPathCommand(paths[0])
SwerveDrive.getInstance().followPathCommand(paths[0]),
new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere())
.onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote())
);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;

public class OnePieceSourceSide extends SequentialCommandGroup {

Expand All @@ -15,6 +16,8 @@ public OnePieceSourceSide(PathPlannerPath... paths) {
ShootRoutine.fromSubwoofer(),
new ArmToFeed(),

new WaitCommand(10),

// Mobility
SwerveDrive.getInstance().followPathCommand(paths[0])
);
Expand Down

0 comments on commit 29d5dc0

Please sign in to comment.