128 make lift use pids

src/main/java/frc/robot/DriverJoystick.java

@@ -9,11 +9,8 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.commands.LiftUpCommand;
-import frc.robot.commands.LeftLiftDownCommand;
-import frc.robot.commands.LeftLiftUpCommand;
 import frc.robot.commands.LiftDownCommand;
-import frc.robot.commands.RightLiftDownCommand;
-import frc.robot.commands.RightLiftUpCommand;
+import frc.robot.commands.LiftDownManualCommand;
 
 public class DriverJoystick extends XboxController1038 {
     // Subsystem Dependencies
@@ -95,14 +92,10 @@ private DriverJoystick() {
                 .onTrue(new InstantCommand(vision::enable0, vision))
                 .onFalse(new InstantCommand(vision::disable0, vision));
 
-        leftBumper.and(rightBumper.negate()).whileTrue(new LeftLiftUpCommand());
-        leftTrigger.and(rightTrigger.negate()).whileTrue(new LeftLiftDownCommand());
+        rightBumper.whileTrue(new LiftUpCommand());
+        rightTrigger.whileTrue(new LiftDownCommand());
 
-        rightBumper.and(leftBumper.negate()).whileTrue(new RightLiftUpCommand());
-        rightTrigger.and(leftTrigger.negate()).whileTrue(new RightLiftDownCommand());
-
-        leftBumper.and(rightBumper).whileTrue(new LiftUpCommand());
-        leftTrigger.and(rightTrigger).whileTrue(new LiftDownCommand());
+        leftTrigger.whileTrue(new LiftDownManualCommand());
     }
 
     /**

src/main/java/frc/robot/Robot.java

@@ -12,7 +12,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 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;
@@ -71,8 +71,10 @@ 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()) {

src/main/java/frc/robot/commands/LiftDownCommand.java

@@ -1,6 +1,8 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.LiftConstants;
 import frc.robot.subsystems.Lift;
 
 public class LiftDownCommand extends Command {
@@ -11,9 +13,18 @@ public LiftDownCommand() {
     }
 
     @Override
-    public void execute() {
-        lift.runLeftDown();
-        lift.runRightDown();
+    public void initialize() {
+        PIDController leftController = lift.getLeftVerticalController();
+        leftController.setP(LiftConstants.kVerticalDownP);
+        leftController.setI(LiftConstants.kVerticalDownI);
+        leftController.setD(LiftConstants.kVerticalDownD);
+        PIDController rightController = lift.getRightVerticalController();
+        rightController.setP(LiftConstants.kVerticalDownP);
+        rightController.setI(LiftConstants.kVerticalDownI);
+        rightController.setD(LiftConstants.kVerticalDownD);
+        lift.enable();
+        lift.setSetpointLeft(LiftConstants.minLiftInches);
+        lift.setSetpointRight(LiftConstants.minLiftInches);
     }
 
     @Override
@@ -23,7 +34,6 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        lift.stopLeftMotor();
-        lift.stopRightMotor();
+        lift.disable();
     }
 }

src/main/java/frc/robot/commands/RightLiftDownCommand.java → src/main/java/frc/robot/commands/LiftDownManualCommand.java

@@ -3,25 +3,26 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Lift;
 
-public class RightLiftDownCommand extends Command {
+public class LiftDownManualCommand extends Command {
     private Lift lift = Lift.getInstance();
 
-    public RightLiftDownCommand() {
+    public LiftDownManualCommand() {
         addRequirements(lift);
     }
 
     @Override
     public void execute() {
+        lift.runLeftDown();
         lift.runRightDown();
     }
 
     @Override
     public boolean isFinished() {
-        return lift.rightLowerLimitReached();
+        return lift.leftLowerLimitReached() && lift.rightLowerLimitReached();
     }
 
     @Override
     public void end(boolean interrupted) {
-        lift.stopRightMotor();
+        lift.stopMotors();
     }
 }

src/main/java/frc/robot/commands/LiftUpCommand.java

@@ -1,6 +1,8 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.LiftConstants;
 import frc.robot.subsystems.Lift;
 
 public class LiftUpCommand extends Command {
@@ -12,14 +14,18 @@ public LiftUpCommand() {
 
     @Override
     public void initialize() {
-        lift.disableLeftRatchet();
-        lift.disableRightRatchet();
-    }
-
-    @Override
-    public void execute() {
-        lift.runLeftUp();
-        lift.runRightUp();
+        lift.disableRatchets();
+        PIDController leftController = lift.getLeftVerticalController();
+        leftController.setP(LiftConstants.kVerticalLeftUpP);
+        leftController.setI(LiftConstants.kVerticalLeftUpI);
+        leftController.setD(LiftConstants.kVerticalLeftUpD);
+        PIDController rightController = lift.getRightVerticalController();
+        rightController.setP(LiftConstants.kVerticalRightUpP);
+        rightController.setI(LiftConstants.kVerticalRightUpI);
+        rightController.setD(LiftConstants.kVerticalRightUpD);
+        lift.enable();
+        lift.setSetpointLeft(LiftConstants.maxLiftInches - 0.5);
+        lift.setSetpointRight(LiftConstants.maxLiftInches);
     }
 
     @Override
@@ -29,9 +35,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        lift.stopLeftMotor();
-        lift.stopRightMotor();
-        lift.enableLeftRatchet();
-        lift.enableRightRatchet();
+        lift.enableRatchets();
+        lift.disable();
     }
 }

src/main/java/frc/robot/constants/LiftConstants.java

@@ -12,8 +12,32 @@ public class LiftConstants {
     public static final double rightRatchetLockPos = 0.55;
     public static final double rightRatchetUnlockPos = 0.65;
 
-    public static final double motorSpeed = 0.55;
-    public static final double backwardsMotorSpeed = -0.9;
+    public static final double upSpeed = 0.55;
+    public static final double downSpeed = -0.9;
 
     public static final double maxExtension = 195;
+    public static final double minExtension = 0;
+    public static final double maxLiftInches = 27;
+    public static final double minLiftInches = 0;
+
+    public static final double maxPower = 1.0;
+
+    public static final double kVerticalDownP = 0.06;
+    public static final double kVerticalDownI = 0.015;
+    public static final double kVerticalDownD = 0.0;
+
+    public static final double kVerticalLeftUpP = 0.001;
+    public static final double kVerticalLeftUpI = 0.0;
+    public static final double kVerticalLeftUpD = 0.0;
+
+    public static final double kVerticalRightUpP = 0.0028;
+    public static final double kVerticalRightUpI = 0.0;
+    public static final double kVerticalRightUpD = 0.0;
+
+    public static final double kErrorP = 0.001;
+    public static final double kErrorI = 0.0;
+    public static final double kErrorD = 0.0;
+
+    public static final double tolerance = 0;
+    public static final double encoderConversion = 1 / (maxExtension / maxLiftInches);
 }

src/main/java/frc/robot/subsystems/Dashboard.java

@@ -10,7 +10,6 @@
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -33,7 +32,6 @@ public class Dashboard extends SubsystemBase {
     // Tabs
     private final ShuffleboardTab driversTab = Shuffleboard.getTab("Drivers");
     private final ShuffleboardTab controlsTab = Shuffleboard.getTab("Controls");
-    private final ShuffleboardTab graphTab = Shuffleboard.getTab("Graph");
     private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault();
 
     // Variables
@@ -102,11 +100,6 @@ private Dashboard() {
                 .withPosition(2, 1)
                 .withSize(4, 3)
                 .withWidget(BuiltInWidgets.kField);
-        BuiltInAccelerometer acc = new BuiltInAccelerometer();
-        graphTab.addDouble("acc", acc::getZ)
-                .withPosition(0, 0)
-                .withSize(2, 2)
-                .withWidget(BuiltInWidgets.kGraph);
 
         PathPlannerLogging.setLogTargetPoseCallback((pose) -> {
             field.getObject("target pose").setPose(pose);

src/main/java/frc/robot/subsystems/DriveTrain.java

@@ -230,4 +230,13 @@ public double getTurnRate() {
     public double getRoll() {
         return -gyro.getRoll().getValue();
     }
+
+    /**
+     * Returns the pitch value of the robot.
+     *
+     * @return the robot's pitch in degrees, from ? to ?
+     */
+    public double getPitch() {
+        return gyro.getPitch().getValue();
+    }
 }