Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

204 teleop place sequence #212

Merged
merged 8 commits into from
Feb 22, 2025
Merged

204 teleop place sequence #212

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
df5a72c
bind
AlexNunemacher Feb 15, 2025
be3fd98
shout out to my brother matthew because i am ray sigma sigma sigma
urayed Feb 20, 2025
36f6164
silly us, spotless
AlexNunemacher Feb 20, 2025
cfe0464
final controlls except auto align and lights but thats tommrows problem
AlexNunemacher Feb 22, 2025
a03b89c
Merge branch 'main' into 204-teleop-place-sequence
mmilunicmobile Feb 22, 2025
8c1a6a0
made small important change
mmilunicmobile Feb 22, 2025
67902cb
p1 open house
AlexNunemacher Feb 22, 2025
db179b3
Updated elastic layout
AlexNunemacher Feb 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions src/main/deploy/elastic-layout.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,8 +162,9 @@
"height": 384.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-april",
"period": 0.06
"topic": "/CameraPublisher/limelight-left",
"period": 0.06,
"rotation_turns": 0
}
},
{
Expand All @@ -174,9 +175,8 @@
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/CameraPublisher/limelight-april/connected",
"topic": "/CameraPublisher/limelight-left/connected",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
"false_color": 4294198070,
"true_icon": "None",
Expand All @@ -191,8 +191,9 @@
"height": 384.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-april-right",
"period": 0.06
"topic": "/CameraPublisher/limelight-right",
"period": 0.06,
"rotation_turns": 0
}
},
{
Expand All @@ -203,9 +204,8 @@
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/CameraPublisher/limelight-april-right/connected",
"topic": "/CameraPublisher/limelight-right/connected",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
"false_color": 4294198070,
"true_icon": "None",
Expand Down
104 changes: 80 additions & 24 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
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.button.InternalButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.lib.controller.LogitechController;
Expand Down Expand Up @@ -92,6 +93,13 @@ public class RobotContainer {

private Supplier<ChassisSpeeds> driverVelocitySupplier;

private Trigger DROP_TRIGGER;
private Trigger AUTO_ALIGNED;
private Trigger USING_AUTO_ALIGN;
private Trigger AUTO_DRIVER_TRIGGER;

private InternalButton normalRelease = new InternalButton();

public RobotContainer() {
if (Robot.isReal()) {
vision =
Expand Down Expand Up @@ -201,6 +209,17 @@ private void configureBindings() {
return drivetrain.driveDriverRelative(driverVelocitySupplier.get());
}));

DROP_TRIGGER = leftDriveController.getTrigger();

final Trigger EJECT_TRIGGER = rightDriveController.getTrigger();
final Trigger ALIGN_TRIGGER = rightDriveController.getBottomThumb();

USING_AUTO_ALIGN = new Trigger(() -> false);

AUTO_ALIGNED = new Trigger(() -> false);

AUTO_DRIVER_TRIGGER = (USING_AUTO_ALIGN.negate().or(AUTO_ALIGNED)).and(DROP_TRIGGER);

// drive.withVelocityX(-leftDriveController.getYAxis().get() *
// GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y
// (forward)
Expand Down Expand Up @@ -354,6 +373,7 @@ private void configureBindings() {
operatorController.getLeftBumper().onTrue(stateManager.setLeftCoralMode());
operatorController.getRightBumper().onTrue(stateManager.setRightCoralMode());
operatorController.getRightTrigger().onTrue(stateManager.setAlgaeMode());

leftDriveController.getRightTopRight().onTrue(stateManager.setArmWristMode());
operatorController
.getLeftJoystick()
Expand All @@ -363,7 +383,7 @@ private void configureBindings() {
LightsSubsystem.LEDSegment.MainStrip.setRainbowAnimation(
1)))); // L3 Rainbow
operatorController
.getLeftTrigger()
.getRightJoystick()
.whileTrue(
Commands.runOnce(
(() ->
Expand All @@ -379,24 +399,37 @@ private void configureBindings() {
ARMWRIST.and(operatorController.getX()).whileTrue(wristSubsystem.turnWristLeft());
ARMWRIST.and(operatorController.getB()).whileTrue(wristSubsystem.turnWristRight());

CORAL.and(operatorController.getY())
.onTrue(stateManager.moveToPosition(Position.L4Prep))
.onFalse(stateManager.moveToPosition(Position.L4));
CORAL.and(operatorController.getX())
.onTrue(stateManager.moveToPosition(Position.L3Prep))
.onFalse(stateManager.moveToPosition(Position.L3));
CORAL.and(operatorController.getB())
.onTrue(stateManager.moveToPosition(Position.L2Prep))
.onFalse(stateManager.moveToPosition(Position.L2));
CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1));
// CORAL.and(operatorController.getY())
// .onTrue(stateManager.moveToPosition(Position.L4Prep))
// .onFalse(stateManager.moveToPosition(Position.L4));
// CORAL.and(operatorController.getX())
// .onTrue(stateManager.moveToPosition(Position.L3Prep))
// .onFalse(stateManager.moveToPosition(Position.L3));
// CORAL.and(operatorController.getB())
// .onTrue(stateManager.moveToPosition(Position.L2Prep))
// .onFalse(stateManager.moveToPosition(Position.L2));
// CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1));

operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Climb));

bindPlaceSeq(CORAL.and(operatorController.getY()), Position.L4Prep, Position.L4, 0.1);

bindPlaceSeq(CORAL.and(operatorController.getX()), Position.L3Prep, Position.L3, 0.1);

bindPlaceSeq(CORAL.and(operatorController.getB()), Position.L2Prep, Position.L2, 0.1);

operatorController.getA().onTrue(stateManager.moveToPosition(Position.L1));

CORAL.and(operatorController.getStart())
.onTrue(stateManager.moveToPosition(Position.Source));

CORAL.and(operatorController.getDPadDown())
.onTrue(stateManager.moveToPosition(Position.Home));
CORAL.and(operatorController.getDPadUp())
.onTrue(stateManager.moveToPosition(Position.HandoffPrep))
.onFalse(stateManager.moveToPosition(Position.Handoff));
.onFalse(stateManager.moveToPosition(Position.HandoffPrep))
.onTrue(stateManager.moveToPosition(Position.Handoff));
CORAL.and(operatorController.getDPadUp()).whileTrue(gripperSubsystem.intakeSpinCoral());

CORAL.and(operatorController.getDPadLeft()).onTrue(chuteSubsystem.moveChuteUp());
CORAL.and(operatorController.getDPadRight()).onTrue(chuteSubsystem.moveChuteDown());

Expand All @@ -409,8 +442,8 @@ private void configureBindings() {
.onTrue(stateManager.moveToPosition(Position.GroundAlgae));
ALGAE.and(operatorController.getDPadDown())
.onTrue(stateManager.moveToPosition(Position.AlgaeHome));
ALGAE.and(operatorController.getDPadUp())
.onTrue(stateManager.moveToPosition(Position.Handoff));
// ALGAE.and(operatorController.getDPadUp())
// .onTrue(stateManager.moveToPosition(Position.Handoff));
ALGAE.and(operatorController.getDPadLeft())
.onTrue(stateManager.moveToPosition(Position.Quick34));
ALGAE.and(operatorController.getDPadRight())
Expand All @@ -434,7 +467,7 @@ private void configureBindings() {
// .whileTrue(intakeSubsystem.openAndRun().alongWith(alignToPiece()));
// rightDriveController.getRightThumb().whileTrue(intakeSubsystem.openAndEject());

CORAL.and(rightDriveController.getBottomThumb())
CORAL.and(rightDriveController.getLeftThumb())
.whileTrue(
gripperSubsystem
.intakeSpinCoral()
Expand All @@ -443,13 +476,13 @@ private void configureBindings() {
.andThen(
Commands.waitUntil(
gripperSubsystem.HAS_PIECE))));
CORAL.and(rightDriveController.getTrigger()).whileTrue(gripperSubsystem.ejectSpinCoral());
CORAL.and(EJECT_TRIGGER).whileTrue(gripperSubsystem.ejectSpinCoral());

ALGAE.and(rightDriveController.getBottomThumb()).onTrue(gripperSubsystem.intakeSpinAlgae());
ALGAE.and(rightDriveController.getTrigger())
ALGAE.and(rightDriveController.getLeftThumb()).onTrue(gripperSubsystem.intakeSpinAlgae());
ALGAE.and(EJECT_TRIGGER)
.and(stateManager.PROCESSOR)
.whileTrue(gripperSubsystem.slowEjectSpinAlgae());
ALGAE.and(rightDriveController.getTrigger())
ALGAE.and(EJECT_TRIGGER)
.and(stateManager.PROCESSOR.negate())
.whileTrue(gripperSubsystem.ejectSpinAlgae());

Expand All @@ -463,19 +496,18 @@ private void configureBindings() {

stateManager
.LEFT_CORAL
.and(leftDriveController.getTrigger())
.and(ALIGN_TRIGGER)
.whileTrue(alignToReef(AligningConstants.leftOffset));

stateManager
.ALGAE
.and(leftDriveController.getTrigger())
.and(ALIGN_TRIGGER)
.whileTrue(alignToReef(AligningConstants.centerOffset));

stateManager
.RIGHT_CORAL
.and(leftDriveController.getTrigger())
.and(ALIGN_TRIGGER)
.whileTrue(alignToReef(AligningConstants.rightOffset));

// Technical Bindings

leftDriveController.getLeftBottomMiddle().onTrue(climberSubsystem.zeroClimberCommand());
Expand Down Expand Up @@ -531,6 +563,30 @@ private void configureBindings() {
// leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand());
}

private void bindPlaceSeq(Trigger button, Position prep, Position end, double timeout) {
(button)
.onTrue(
(stateManager
.moveToPosition(prep)
.until(AUTO_DRIVER_TRIGGER)
.andThen(
stateManager
.moveToPosition(end)
// .alongWith(
//
// Commands.waitSeconds(timeout)
//
// .andThen(
//
// gripperSubsystem
//
// .ejectSpinCoral()))
.until(AUTO_DRIVER_TRIGGER.negate())))
.repeatedly()
.beforeStarting(() -> normalRelease.setPressed(false))
.finallyDo(() -> normalRelease.setPressed(true)));
}

private double deadband(double value, double deadband) {
if (value <= deadband && -deadband <= value) {
return 0;
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/constants/TunerConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ public class TunerConstants {

// The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot
private static final Current kSlipCurrent = Amps.of(60.0);
private static final Current kSlipCurrent = Amps.of(102);

// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
Expand All @@ -80,7 +80,7 @@ public class TunerConstants {

// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6.218);

// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ public static enum Position {
AlgaeHome(100, 2.15, -1.58, ChuteUpNull),
AlgaeHomeNull(0.0, 0.0, 0.0, AlgaeHome, TRUE, FALSE),
// Climb(170, 0, 1.58, ChuteUpNull),
Processor(65, 1.58, -1.58, AlgaeHomeNull),
Processor(80, 1.58, -1.58, AlgaeHomeNull),

// IcecreamCoral(170, 0, 1.58, AlgaeHome),
// IcecreamAlgae(170, 0, 1.58, AlgaeHome),
Expand All @@ -182,6 +182,7 @@ public static enum Position {
StartPrepPrep(140, 0, 1.58, StartPrepPrepPrep),
StartPrep(140, 0, -1.58, StartPrepPrep),
Start(0, 0, -1.58, StartPrep),
Climb(10, 0, -1.58, StartPrep),
Tunable(170, 0, 1.58, CenterZoneNull, FALSE);

private double elevatorHeight;
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -559,8 +559,7 @@ public void periodic() {
"Drive/setpointChassisSpeeds",
m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds());

ChassisSpeeds speedsPreview =
m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds();
ChassisSpeeds speedsPreview = getChassisSpeeds();

double currentSpeed =
Math.hypot(speedsPreview.vxMetersPerSecond, speedsPreview.vyMetersPerSecond);
Expand Down
Loading