Skip to content
This repository has been archived by the owner on Mar 4, 2024. It is now read-only.

Commit

Permalink
Feat: 2.5 Piece Taxi Cone Vision works
Browse files Browse the repository at this point in the history
  • Loading branch information
@sanashah007
sanashah007 committed Apr 5, 2023
1 parent c1913a9 commit 1fe19b5
Show file tree
Hide file tree
Showing 6 changed files with 140 additions and 48 deletions.
6 changes: 3 additions & 3 deletions src/main/deploy/pathplanner/BottomGrid2PieceTaxiVision.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,15 +99,15 @@
},
{
"anchorPoint": {
"x": 1.691366753254689,
"x": 1.55,
"y": 1.0457402965965006
},
"prevControl": {
"x": 2.3305118957485313,
"x": 2.1891451424938424,
"y": 1.0856858486827012
},
"nextControl": null,
"holonomicAngle": 180.0,
"holonomicAngle": 178.89191179397594,
"isReversal": false,
"velOverride": null,
"isLocked": false,
Expand Down
107 changes: 91 additions & 16 deletions src/main/deploy/pathplanner/BottomGrid2_5PieceTaxiConeVision.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,15 @@
"waypoints": [
{
"anchorPoint": {
"x": 1.8710528497445884,
"y": 0.4647735250331603
"x": 1.8484512516341418,
"y": 0.5042383195642101
},
"prevControl": null,
"nextControl": {
"x": 2.4412241867777134,
"y": 0.10510098638034204
"x": 2.62133701044165,
"y": 0.5042383195642101
},
"holonomicAngle": 179.52224912952565,
"holonomicAngle": 180.0,
"isReversal": false,
"velOverride": null,
"isLocked": false,
Expand All @@ -24,18 +24,43 @@
},
{
"anchorPoint": {
"x": 6.21035572686416,
"y": 0.8017943092965504
"x": 4.505776740190486,
"y": 0.6801856912329646
},
"prevControl": {
"x": 5.941248284591866,
"y": 0.8017943092965504
"x": 4.238533158369918,
"y": 0.6801856912329646
},
"nextControl": {
"x": 5.941248284591866,
"y": 0.8017943092965504
"x": 4.901374510269792,
"y": 0.6801856912329646
},
"holonomicAngle": 180.0,
"isReversal": false,
"velOverride": null,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 6.0,
"y": 0.8017943092965504
},
"prevControl": {
"x": 4.8265521610492375,
"y": 0.7562044467309281
},
"nextControl": {
"x": 4.8265521610492375,
"y": 0.7562044467309281
},
"holonomicAngle": -170.02766511786143,
"isReversal": true,
"velOverride": null,
"isLocked": false,
Expand Down Expand Up @@ -74,15 +99,65 @@
},
{
"anchorPoint": {
"x": 6.547541871955501,
"y": 1.4577746279287969
"x": 3.970040720801781,
"y": 0.9422815685320645
},
"prevControl": {
"x": 3.1167894796591717,
"y": 0.9422815685320645
},
"nextControl": {
"x": 4.810973975462737,
"y": 0.9422815685320645
},
"holonomicAngle": 180.0,
"isReversal": false,
"velOverride": null,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 5.337693877188602,
"y": 0.9912420275052416
},
"prevControl": {
"x": 5.072919411667544,
"y": 0.9681306831716776
},
"nextControl": {
"x": 5.602468342709661,
"y": 1.0143533718388054
},
"holonomicAngle": -92.79608267617628,
"isReversal": false,
"velOverride": null,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 6.542130293829797,
"y": 1.4879648200327484
},
"prevControl": {
"x": 5.767665810946437,
"y": -0.2123554796901983
"x": 6.081546069342139,
"y": 0.8677229571973278
},
"nextControl": null,
"holonomicAngle": -123.87016329772545,
"holonomicAngle": -303.0,
"isReversal": false,
"velOverride": null,
"isLocked": false,
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2023/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,7 @@ private void initAutos() {
manager.addAuto(new AutoTopGrid2PieceVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight));
manager.addAuto(new AutoBottomGrid2PieceTaxiVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight));
manager.addAuto(new AutoBottomGrid2_5PieceTaxiVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight));
manager.addAuto(new AutoBottomGrid2_5PieceTaxiConeVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight));
manager.addAuto(new AutoBottomGrid2_5PieceTaxiConeVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight, LimelightManager.getInstance().frontLimelight));
//manager.addAuto(new Auto2PieceTaxiConeVision(drivetrain, arm, intake, ScoringGridLocation.TOP_GRID));
// manager.addAuto(new Auto2PieceTaxi(drivetrain, arm, intake, ScoringGridLocation.BOTTOM_GRID));
// manager.addAuto("MiddleGrid1PieceSideBalance", new Auto1PieceSideBalance(drivetrain, arm, intake));
Expand Down
57 changes: 31 additions & 26 deletions src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiConeVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import com.pathplanner.lib.PathConstraints;
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.ProxyCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import org.team1540.robot2023.Constants;
Expand All @@ -11,10 +12,12 @@
import org.team1540.robot2023.commands.arm.ResetArmPositionCommand;
import org.team1540.robot2023.commands.arm.SetArmPosition;
import org.team1540.robot2023.commands.drivetrain.Drivetrain;
import org.team1540.robot2023.commands.grabber.GrabberAggressiveCommand;
import org.team1540.robot2023.commands.grabber.GrabberIntakeCommand;
import org.team1540.robot2023.commands.grabber.GrabberOuttakeCommand;
import org.team1540.robot2023.commands.grabber.WheeledGrabber;
import org.team1540.robot2023.commands.vision.DriveToGamePiece;
import org.team1540.robot2023.commands.vision.DriveToGamePieceReverse;
import org.team1540.robot2023.commands.vision.TurnToGamePiece;
import org.team1540.robot2023.utils.AutoCommand;
import org.team1540.robot2023.utils.Limelight;
Expand All @@ -23,54 +26,56 @@
import java.util.List;

public class AutoBottomGrid2_5PieceTaxiConeVision extends AutoCommand {
public AutoBottomGrid2_5PieceTaxiConeVision(Drivetrain drivetrain, Arm arm, WheeledGrabber intake, Limelight limelight) {
public AutoBottomGrid2_5PieceTaxiConeVision(Drivetrain drivetrain, Arm arm, WheeledGrabber intake, Limelight limelight, Limelight frontLimelight) {
List<Command> pathCommands = getPathPlannerDriveCommandGroup(drivetrain,"BottomGrid2_5PieceTaxiConeVision", new PathConstraints[]{
new PathConstraints(2,1),
new PathConstraints(4,2)
}, true);
limelight.setPipeline(Limelight.Pipeline.GAME_PIECE);
setName("BottomGrid2.5PieceTaxiConeVision");
addCommands(
Commands.deadline(
new SetArmPosition(arm, Constants.Auto.highCube.approach),
Commands.sequence(
new ProxyCommand(() -> new WaitCommand((arm.timeToState(Constants.Auto.highCube.approach)-150)/1000)),
new GrabberOuttakeCommand(intake, 0.5)
)
Commands.deadline(
new SetArmPosition(arm, Constants.Auto.highCone.approach),
new GrabberAggressiveCommand(intake)
),
new SetArmPosition(arm, AutoHybrid.catchNull(Constants.Auto.highCone.score)).unless(() -> Constants.Auto.highCone.score == null),
new GrabberOuttakeCommand(intake, 1).withTimeout(0.1),
new SetArmPosition(arm, AutoHybrid.catchNull(Constants.Auto.highCone.retreat)).unless(() -> Constants.Auto.highCone.retreat == null),
Commands.parallel(
new GrabberIntakeCommand(intake),
new GrabberIntakeCommand(intake),
Commands.sequence(
Commands.parallel(
pathCommands.get(0),
Commands.sequence(
new ResetArmPositionCommand(arm),
new PivotCommand(arm, Constants.Auto.armDownBackwards)
)
),
Commands.sequence(
Commands.parallel(
pathCommands.get(0),
Commands.sequence(
new ResetArmPositionCommand(arm),
new PivotCommand(arm, Constants.Auto.armDownBackwards)
)
),
Commands.sequence(
new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CUBE ),
new DriveToGamePiece(drivetrain, () -> Constants.Auto.autoDriveDistance)
),
Commands.parallel(
new SetArmPosition(arm, Constants.Auto.midCube.approach),
pathCommands.get(1)
)
new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CUBE ),
new DriveToGamePiece(drivetrain, () -> Constants.Auto.autoDriveDistance)
)
)
),
Commands.parallel(
new SetArmPosition(arm, Constants.Auto.midCube.approach),
pathCommands.get(1)
),
new GrabberOuttakeCommand(intake,1).withTimeout(0.1),
new InstantCommand(drivetrain::updateWithScoringApriltags),
Commands.parallel(
new InstantCommand(()-> frontLimelight.setPipeline(Limelight.Pipeline.GAME_PIECE)),
Commands.sequence(
new ResetArmPositionCommand(arm),
new PivotCommand(arm, Constants.Auto.armDownBackwards)
new SetArmPosition(arm, Constants.Auto.armDown)
),
pathCommands.get(2)
),
Commands.parallel(
new GrabberIntakeCommand(intake),
Commands.parallel(
new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CONE ),
new DriveToGamePiece(drivetrain, () -> Constants.Auto.autoDriveDistance)
new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CONE, frontLimelight)
// new DriveToGamePieceReverse(drivetrain, () -> Constants.Auto.autoDriveDistance)
)
)
// new AutoGridScore(drivetrain, arm, Constants.Auto.midCube.withPolePosition(PolePosition.CENTER), intake, null, false)
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ public AutoBottomGrid2_5PieceTaxiVision(Drivetrain drivetrain, Arm arm, WheeledG
new SetArmPosition(arm, Constants.Auto.highCube.approach),
Commands.sequence(
new ProxyCommand(() -> new WaitCommand((arm.timeToState(Constants.Auto.highCube.approach)-150)/1000)),
new GrabberOuttakeCommand(intake, 0.5)
new GrabberOuttakeCommand(intake)
)
),
Commands.parallel(
Expand Down
14 changes: 13 additions & 1 deletion src/main/java/org/team1540/robot2023/commands/vision/TurnToGamePiece.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,10 +70,22 @@ public TurnToGamePiece(Drivetrain drivetrain, CommandXboxController controller,
// }

}
public TurnToGamePiece(Drivetrain drivetrain, CommandXboxController controller, GamePiece gamepiece, Limelight limelight){
this.drivetrain = drivetrain;
this.controller = controller;
this.angleSupplier = drivetrain::getRawGyroAngle;
this.gamepiece = gamepiece;
this.limelight = limelight;
// if(controller != null){
// addRequirements(drivetrain);
// }

}


@Override
public void initialize() {
//startTime = System.currentTimeMillis();
startTime = System.currentTimeMillis();
limelight.setPipeline(Limelight.Pipeline.GAME_PIECE);
pid.enableContinuousInput(-180, 180);
// updatePID();
Expand Down

0 comments on commit 1fe19b5

Please sign in to comment.