From 351753cafcf58a6a55d4339a5e16ff9d504f05e4 Mon Sep 17 00:00:00 2001 From: Griffin Date: Tue, 19 Mar 2024 18:26:44 -0400 Subject: [PATCH 1/2] added moving the lift down at start of auton before moving it up --- src/main/java/frc/robot/Robot.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b81a0f0..975ff1ad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,9 +10,11 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.autons.Auton; import frc.robot.autons.AutonSelector; +import frc.robot.commands.LiftDownCommand; import frc.robot.commands.LiftUpCommand; import frc.robot.constants.SwerveModuleConstants; import frc.robot.subsystems.Dashboard; @@ -70,7 +72,9 @@ public void disabledExit() { @Override public void autonomousInit() { - new LiftUpCommand().schedule(); + new SequentialCommandGroup( + new LiftDownCommand(), + new LiftUpCommand()).schedule(); driveTrain.zeroHeading(); autonomousCommand = autonSelector.chooseAuton(); // if (DriverStation.isFMSAttached()) { From e1a2ce86652562c9bdec6076de7b2ffb19373ada Mon Sep 17 00:00:00 2001 From: Griffin Date: Tue, 19 Mar 2024 18:45:04 -0400 Subject: [PATCH 2/2] fixed method name and added if >and = --- src/main/java/frc/robot/commands/LiftUpCommand.java | 2 +- src/main/java/frc/robot/subsystems/Lift.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index 04b71c21..7c3c43ce 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -24,7 +24,7 @@ public void execute() { @Override public boolean isFinished() { - return !lift.isLiftUp(); + return lift.isLiftUp(); } @Override diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index 45570aad..7ae301dd 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -142,8 +142,8 @@ public void runRightUp() { } public boolean isLiftUp() { - return rightLiftEncoder.getPosition() < LiftConstants.maxExtension - && leftLiftEncoder.getPosition() < LiftConstants.maxExtension; + return rightLiftEncoder.getPosition() >= LiftConstants.maxExtension + && leftLiftEncoder.getPosition() >= LiftConstants.maxExtension; } public boolean isRightLiftUp() {