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

Commit

Permalink
feat: BottomGrid2_5TaxiConeVision slay
Browse files Browse the repository at this point in the history
  • Loading branch information
@sanashah007
sanashah007 committed Apr 4, 2023
1 parent adc4229 commit c1913a9
Show file tree
Hide file tree
Showing 3 changed files with 182 additions and 1 deletion.
99 changes: 99 additions & 0 deletions src/main/deploy/pathplanner/BottomGrid2_5PieceTaxiConeVision.path
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
{
"waypoints": [
{
"anchorPoint": {
"x": 1.8710528497445884,
"y": 0.4647735250331603
},
"prevControl": null,
"nextControl": {
"x": 2.4412241867777134,
"y": 0.10510098638034204
},
"holonomicAngle": 179.52224912952565,
"isReversal": false,
"velOverride": null,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 6.21035572686416,
"y": 0.8017943092965504
},
"prevControl": {
"x": 5.941248284591866,
"y": 0.8017943092965504
},
"nextControl": {
"x": 5.941248284591866,
"y": 0.8017943092965504
},
"holonomicAngle": 180.0,
"isReversal": true,
"velOverride": null,
"isLocked": false,
"isStopPoint": true,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 1.691366753254689,
"y": 1.0457402965965006
},
"prevControl": {
"x": 2.3305118957485313,
"y": 1.0856858486827012
},
"nextControl": {
"x": 2.3305118957485313,
"y": 1.0856858486827012
},
"holonomicAngle": 180.0,
"isReversal": true,
"velOverride": null,
"isLocked": false,
"isStopPoint": true,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 6.547541871955501,
"y": 1.4577746279287969
},
"prevControl": {
"x": 5.767665810946437,
"y": -0.2123554796901983
},
"nextControl": null,
"holonomicAngle": -123.87016329772545,
"isReversal": false,
"velOverride": null,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
}
],
"markers": []
}
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 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
82 changes: 82 additions & 0 deletions src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiConeVision.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
package org.team1540.robot2023.commands.auto;

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.ProxyCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import org.team1540.robot2023.Constants;
import org.team1540.robot2023.commands.arm.Arm;
import org.team1540.robot2023.commands.arm.PivotCommand;
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.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.TurnToGamePiece;
import org.team1540.robot2023.utils.AutoCommand;
import org.team1540.robot2023.utils.Limelight;
import org.team1540.robot2023.utils.PolePosition;

import java.util.List;

public class AutoBottomGrid2_5PieceTaxiConeVision extends AutoCommand {
public AutoBottomGrid2_5PieceTaxiConeVision(Drivetrain drivetrain, Arm arm, WheeledGrabber intake, Limelight limelight) {
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.parallel(
new GrabberIntakeCommand(intake),
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 GrabberOuttakeCommand(intake,1).withTimeout(0.1),
Commands.parallel(
Commands.sequence(
new ResetArmPositionCommand(arm),
new PivotCommand(arm, Constants.Auto.armDownBackwards)
),
pathCommands.get(2)
),
Commands.parallel(
new GrabberIntakeCommand(intake),
Commands.parallel(
new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CONE ),
new DriveToGamePiece(drivetrain, () -> Constants.Auto.autoDriveDistance)
)
)
// new AutoGridScore(drivetrain, arm, Constants.Auto.midCube.withPolePosition(PolePosition.CENTER), intake, null, false)

);
}


}

0 comments on commit c1913a9

Please sign in to comment.