lift runs with new code (WIP)

LakotaRobotics1038 · Mar 21, 2024 · bc2c99a · bc2c99a
1 parent 80177ce
commit bc2c99a
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.commands.LiftUpCommand;
 import frc.robot.commands.LiftDownCommand;
+import frc.robot.commands.LiftDownManualCommand;
 
 public class DriverJoystick extends XboxController1038 {
     // Subsystem Dependencies
@@ -94,8 +95,7 @@ private DriverJoystick() {
         rightBumper.whileTrue(new LiftUpCommand());
         rightTrigger.whileTrue(new LiftDownCommand());
 
-        leftBumper.and(rightBumper).whileTrue(new LiftUpCommand());
-        leftTrigger.and(rightTrigger).whileTrue(new LiftDownCommand());
+        leftTrigger.whileTrue(new LiftDownManualCommand());
     }
 
     /**

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -8,13 +8,11 @@
 import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.math.geometry.Pose2d;
 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.LiftDownManualCommand;
 import frc.robot.commands.LiftUpCommand;
 import frc.robot.constants.SwerveModuleConstants;
 import frc.robot.subsystems.Dashboard;
@@ -73,8 +71,9 @@ public void disabledExit() {
     @Override
     public void autonomousInit() {
         new SequentialCommandGroup(
-        new LiftDownCommand(),
-        new LiftUpCommand()).schedule();
+                new LiftDownManualCommand(),
+                new LiftUpCommand())
+                .schedule();
         driveTrain.zeroHeading();
         autonomousCommand = autonSelector.chooseAuton();
         // if (DriverStation.isFMSAttached()) {

diff --git a/src/main/java/frc/robot/commands/LiftDownManualCommand.java b/src/main/java/frc/robot/commands/LiftDownManualCommand.java
@@ -0,0 +1,28 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Lift;
+
+public class LiftDownManualCommand extends Command {
+    private Lift lift = Lift.getInstance();
+
+    public LiftDownManualCommand() {
+        addRequirements(lift);
+    }
+
+    @Override
+    public void execute() {
+        lift.runLeftDown();
+        lift.runRightDown();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return lift.leftLowerLimitReached() && lift.rightLowerLimitReached();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        lift.stopMotors();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java
@@ -24,6 +24,7 @@ public class Dashboard extends SubsystemBase {
     private DriveTrain driveTrain = DriveTrain.getInstance();
     private Scoring scoring = Scoring.getInstance();
     private Vision vision = Vision.getInstance();
+    private Lift lift = Lift.getInstance();
 
     // Choosers
     private SendableChooser<AutonChoices> autoChooser = new SendableChooser<>();
@@ -32,6 +33,7 @@ public class Dashboard extends SubsystemBase {
     // Tabs
     private final ShuffleboardTab driversTab = Shuffleboard.getTab("Drivers");
     private final ShuffleboardTab controlsTab = Shuffleboard.getTab("Controls");
+    private final ShuffleboardTab tempTab = Shuffleboard.getTab("Temp");
     private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault();
 
     // Variables
@@ -101,6 +103,11 @@ private Dashboard() {
                 .withSize(4, 3)
                 .withWidget(BuiltInWidgets.kField);
 
+        tempTab.add("Vert", lift.getVerticalController())
+                .withPosition(0, 0);
+        tempTab.add("Err", lift.getErrorController())
+                .withPosition(1, 0);
+
         PathPlannerLogging.setLogTargetPoseCallback((pose) -> {
             field.getObject("target pose").setPose(pose);
         });

diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java
@@ -75,6 +75,9 @@ private Lift() {
         leftLiftMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeoCurrent);
         rightLiftMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeoCurrent);
 
+        leftLiftEncoder.setPositionConversionFactor(LiftConstants.encoderConversion);
+        rightLiftEncoder.setPositionConversionFactor(LiftConstants.encoderConversion);
+
         leftLiftMotor.burnFlash();
         rightLiftMotor.burnFlash();
 
@@ -126,7 +129,7 @@ public boolean rightRatchetUnlocked() {
     private void runLeft(double speed) {
         speed = MathUtil.clamp(speed, LiftConstants.downSpeed, LiftConstants.upSpeed);
         if (speed > 0 && this.leftRatchetUnlocked()) {
-            if (!(leftLiftEncoder.getPosition() < LiftConstants.maxExtension)) {
+            if (leftLiftEncoder.getPosition() < LiftConstants.maxExtension) {
                 leftLiftMotor.set(LiftConstants.maxExtension);
             } else {
                 leftLiftMotor.stopMotor();
@@ -144,7 +147,7 @@ private void runLeft(double speed) {
     private void runRight(double speed) {
         speed = MathUtil.clamp(speed, LiftConstants.downSpeed, LiftConstants.upSpeed);
         if (speed > 0 && this.rightRatchetUnlocked()) {
-            if ((!(rightLiftEncoder.getPosition() < LiftConstants.maxExtension))) {
+            if (rightLiftEncoder.getPosition() < LiftConstants.maxExtension) {
                 rightLiftMotor.set(speed);
             } else {
                 rightLiftMotor.stopMotor();
@@ -200,6 +203,7 @@ public void periodic() {
 
             double left = pitch > 0 ? power + clampedError : power;
             double right = pitch < 0 ? power - clampedError : power;
+
             runLeft(left);
             runRight(right);
         }
@@ -273,8 +277,8 @@ public double getRightPosition() {
      * @return whether lift is up or not
      */
     public boolean isLiftUp() {
-        return rightLiftEncoder.getPosition() < LiftConstants.maxExtension
-                && leftLiftEncoder.getPosition() < LiftConstants.maxExtension;
+        return rightLiftEncoder.getPosition() >= LiftConstants.maxExtension
+                && leftLiftEncoder.getPosition() >= LiftConstants.maxExtension;
     }
 
     /**