Merge pull request #134 from LakotaRobotics1038/make-lift-go-down-at-…

…start-of-match Make lift go down at start of match
LakotaRobotics1038 · Mar 19, 2024 · 854e26e · 854e26e
2 parents 4f1c16d + e1a2ce8
commit 854e26e
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 4 deletions.
diff --git 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()) {

diff --git 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
@@ -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() {