diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 24056b34..cedb5264 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -168,14 +168,13 @@ private void configureDriverBindings() { .onTrue(new AmperScoreTrap()) .onFalse(new AmperStop()); - driver.getDPadUp() - .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0))); driver.getDPadRight() - .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90))); - driver.getDPadDown() - .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180))); - driver.getDPadLeft() - .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270))); + .whileTrue(new TrapScoreRoutine()) + // .onFalse(new AmperToHeight(Lift.MIN_HEIGHT)) + .onFalse(new AmperStop()); + + driver.getDPadUp() + .onTrue(new SwerveDriveResetHeading()); driver.getTopButton() // on command start diff --git a/src/main/java/com/stuypulse/robot/commands/TrapScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/TrapScoreRoutine.java new file mode 100644 index 00000000..a2eb75a6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/TrapScoreRoutine.java @@ -0,0 +1,26 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands; + +import com.stuypulse.robot.commands.amper.AmperScoreTrap; +import com.stuypulse.robot.commands.amper.AmperToHeight; +import com.stuypulse.robot.commands.conveyor.ConveyorToAmp; +import com.stuypulse.robot.constants.Settings.Amper.Lift; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class TrapScoreRoutine extends SequentialCommandGroup { + + public TrapScoreRoutine() { + addCommands( + // NOTE: will not work if lift is at safe height, maybe this shouldn't run + new ConveyorToAmp(), + AmperToHeight.untilDone(Lift.TRAP_SCORE_HEIGHT), + new AmperScoreTrap() + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 5c20a32b..75359d7d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -88,7 +88,7 @@ public interface Amper { public interface Score { double AMP_SPEED = 1.0; - double TRAP_SPEED = 0.5; + double TRAP_SPEED = 0.3; double FROM_CONVEYOR_SPEED = 0.35; double TO_CONVEYOR_SPEED = 1.0; @@ -103,7 +103,7 @@ public interface Lift { double CARRIAGE_MASS = 10; // kg double MIN_HEIGHT = 0; - double MAX_HEIGHT = Units.inchesToMeters(27.0); // amp 14.75 + double MAX_HEIGHT = Units.inchesToMeters(27.0 - 2.0); // amp 14.75 double VISUALIZATION_MIN_LENGTH = 0.5; Rotation2d ANGLE_TO_GROUND = Rotation2d.fromDegrees(68.02); @@ -114,7 +114,7 @@ public interface Lift { double ACCEL_LIMIT = 2.0; double AMP_SCORE_HEIGHT = 0.34; - double TRAP_SCORE_HEIGHT = 0.60; //TODO: Tune this value + double TRAP_SCORE_HEIGHT = MAX_HEIGHT; //TODO: Tune this value public interface Encoder { double GEARING = 1.0 / 9.0;