From 9c3d0135e059057d891f7ea9943cef0e4e4773db Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:32:57 -0400 Subject: [PATCH 01/18] Deleted unnessasary commands for PID lift --- .../robot/commands/LeftLiftDownCommand.java | 27 --------------- .../frc/robot/commands/LeftLiftUpCommand.java | 33 ------------------- .../robot/commands/RightLiftDownCommand.java | 27 --------------- .../robot/commands/RightLiftUpCommand.java | 33 ------------------- 4 files changed, 120 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/LeftLiftDownCommand.java delete mode 100644 src/main/java/frc/robot/commands/LeftLiftUpCommand.java delete mode 100644 src/main/java/frc/robot/commands/RightLiftDownCommand.java delete mode 100644 src/main/java/frc/robot/commands/RightLiftUpCommand.java diff --git a/src/main/java/frc/robot/commands/LeftLiftDownCommand.java b/src/main/java/frc/robot/commands/LeftLiftDownCommand.java deleted file mode 100644 index 06ac6bab..00000000 --- a/src/main/java/frc/robot/commands/LeftLiftDownCommand.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lift; - -public class LeftLiftDownCommand extends Command { - private Lift lift = Lift.getInstance(); - - public LeftLiftDownCommand() { - addRequirements(lift); - } - - @Override - public void execute() { - lift.runLeftDown(); - } - - @Override - public boolean isFinished() { - return lift.leftLowerLimitReached(); - } - - @Override - public void end(boolean interrupted) { - lift.stopLeftMotor(); - } -} diff --git a/src/main/java/frc/robot/commands/LeftLiftUpCommand.java b/src/main/java/frc/robot/commands/LeftLiftUpCommand.java deleted file mode 100644 index 828957aa..00000000 --- a/src/main/java/frc/robot/commands/LeftLiftUpCommand.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lift; - -public class LeftLiftUpCommand extends Command { - private Lift lift = Lift.getInstance(); - - public LeftLiftUpCommand() { - addRequirements(lift); - } - - @Override - public void initialize() { - lift.disableLeftRatchet(); - } - - @Override - public void execute() { - lift.runLeftUp(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - lift.stopLeftMotor(); - lift.enableLeftRatchet(); - } -} diff --git a/src/main/java/frc/robot/commands/RightLiftDownCommand.java b/src/main/java/frc/robot/commands/RightLiftDownCommand.java deleted file mode 100644 index 37dccb01..00000000 --- a/src/main/java/frc/robot/commands/RightLiftDownCommand.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lift; - -public class RightLiftDownCommand extends Command { - private Lift lift = Lift.getInstance(); - - public RightLiftDownCommand() { - addRequirements(lift); - } - - @Override - public void execute() { - lift.runRightDown(); - } - - @Override - public boolean isFinished() { - return lift.rightLowerLimitReached(); - } - - @Override - public void end(boolean interrupted) { - lift.stopRightMotor(); - } -} diff --git a/src/main/java/frc/robot/commands/RightLiftUpCommand.java b/src/main/java/frc/robot/commands/RightLiftUpCommand.java deleted file mode 100644 index ab9183f0..00000000 --- a/src/main/java/frc/robot/commands/RightLiftUpCommand.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Lift; - -public class RightLiftUpCommand extends Command { - private Lift lift = Lift.getInstance(); - - public RightLiftUpCommand() { - addRequirements(lift); - } - - @Override - public void initialize() { - lift.disableRightRatchet(); - } - - @Override - public void execute() { - lift.runRightUp(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - lift.stopRightMotor(); - lift.enableRightRatchet(); - } -} From d2a7ab1e8582b90cd087f673b93ec78d6fab0794 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:33:23 -0400 Subject: [PATCH 02/18] updated driverJoystick with new PID lift --- src/main/java/frc/robot/DriverJoystick.java | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index 1173f399..6b6ad3ca 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -9,11 +9,7 @@ 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; public class DriverJoystick extends XboxController1038 { // Subsystem Dependencies @@ -95,11 +91,8 @@ 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.and(leftBumper.negate()).whileTrue(new RightLiftUpCommand()); - rightTrigger.and(leftTrigger.negate()).whileTrue(new RightLiftDownCommand()); + rightBumper.and(leftBumper.negate()).whileTrue(new LiftUpCommand()); + rightTrigger.and(leftTrigger.negate()).whileTrue(new LiftDownCommand()); leftBumper.and(rightBumper).whileTrue(new LiftUpCommand()); leftTrigger.and(rightTrigger).whileTrue(new LiftDownCommand()); From ca9dc8ce7b252a7cacacb3c0311686a215bde8ca Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:33:39 -0400 Subject: [PATCH 03/18] updated commands for new PID lift --- .../java/frc/robot/commands/LiftDownCommand.java | 6 ++---- src/main/java/frc/robot/commands/LiftUpCommand.java | 12 ++++-------- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftDownCommand.java b/src/main/java/frc/robot/commands/LiftDownCommand.java index 04fcfcbe..6d1489da 100644 --- a/src/main/java/frc/robot/commands/LiftDownCommand.java +++ b/src/main/java/frc/robot/commands/LiftDownCommand.java @@ -12,8 +12,7 @@ public LiftDownCommand() { @Override public void execute() { - lift.runLeftDown(); - lift.runRightDown(); + lift.runLiftDown(); } @Override @@ -23,7 +22,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - lift.stopLeftMotor(); - lift.stopRightMotor(); + lift.stopMotors(); } } diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index 04b71c21..fb6833c6 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -12,14 +12,12 @@ public LiftUpCommand() { @Override public void initialize() { - lift.disableLeftRatchet(); - lift.disableRightRatchet(); + lift.disableRatchets(); } @Override public void execute() { - lift.runLeftUp(); - lift.runRightUp(); + lift.runLiftUp(); } @Override @@ -29,9 +27,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - lift.stopLeftMotor(); - lift.stopRightMotor(); - lift.enableLeftRatchet(); - lift.enableRightRatchet(); + lift.stopMotors(); + lift.enableRatchets(); } } From 060ad10d80ec429f0e8b9f7ffd284169bb83b26d Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:33:49 -0400 Subject: [PATCH 04/18] added new constants for pid lift --- src/main/java/frc/robot/constants/LiftConstants.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index fc9e1fb2..1158ee3f 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -16,4 +16,13 @@ public class LiftConstants { public static final double backwardsMotorSpeed = -0.9; public static final double maxExtension = 195; + + public static final double maxPower = 1.0; + + public static final double kP = 0.01; + public static final double kI = 0.0; + public static final double kD = 0.0; + + public static final double tolerance = 2.0; + public static final double encoderConversion = 0.0; } \ No newline at end of file From 3e83d769a023760dc37b77628c380c065bac2780 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:34:00 -0400 Subject: [PATCH 05/18] made lift pid still tuning needed --- src/main/java/frc/robot/subsystems/Lift.java | 115 ++++++------------- 1 file changed, 38 insertions(+), 77 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index faa9c2a4..80534a23 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -6,12 +6,14 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkLimitSwitch; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; import frc.robot.constants.LiftConstants; import frc.robot.constants.NeoMotorConstants; -public final class Lift extends SubsystemBase { +public final class Lift extends PIDSubsystem { private CANSparkMax leftLiftMotor = new CANSparkMax(LiftConstants.leftMotorPort, MotorType.kBrushless); private CANSparkMax rightLiftMotor = new CANSparkMax(LiftConstants.rightMotorPort, MotorType.kBrushless); private RelativeEncoder leftLiftEncoder = leftLiftMotor.getEncoder(); @@ -42,6 +44,9 @@ public static Lift getInstance() { * follow the left. Burns these settings to the flash of each motor. */ private Lift() { + super(new PIDController(LiftConstants.kP, LiftConstants.kI, LiftConstants.kD)); + getController().setTolerance(LiftConstants.tolerance); + getController().enableContinuousInput(0, LiftConstants.encoderConversion); leftLiftMotor.restoreFactoryDefaults(); rightLiftMotor.restoreFactoryDefaults(); @@ -51,6 +56,8 @@ private Lift() { leftLimitSwitch.enableLimitSwitch(true); rightLimitSwitch.enableLimitSwitch(true); + rightLiftMotor.follow(leftLiftMotor); + leftLiftMotor.setInverted(false); rightLiftMotor.setInverted(true); @@ -64,79 +71,35 @@ private Lift() { rightLiftMotor.burnFlash(); } - /** - * Enables the left lift ratchet (sets them to a constant maximum extension). - */ - public void enableLeftRatchet() { - leftRatchetServo.set(LiftConstants.leftRatchetLockPos); - } - - /** - * Enables the right lift ratchet (sets them to a constant maximum extension). - */ - public void enableRightRatchet() { + public void enableRatchets() { rightRatchetServo.set(LiftConstants.rightRatchetLockPos); + leftRatchetServo.set(LiftConstants.leftRatchetLockPos); } - /** - * Disables the left lift ratchet (sets them to a constant minimum extension). - */ - public void disableLeftRatchet() { + public void disableRatchets() { leftRatchetServo.set(LiftConstants.leftRatchetUnlockPos); - } - - /** - * Disables the right lift ratchet (sets them to a constant minimum extension). - */ - public void disableRightRatchet() { rightRatchetServo.set(LiftConstants.rightRatchetUnlockPos); - } - /** - * Determines if the left ratchet is in the unlocked position - * - * @return is the ratchet unlocked - */ - public boolean leftRatchetUnlocked() { - return leftRatchetServo.get() == LiftConstants.leftRatchetUnlockPos; } - /** - * Determines if the right ratchet is in the unlocked position - * - * @return is the ratchet unlocked - */ - public boolean rightRatchetUnlocked() { - return rightRatchetServo.get() == LiftConstants.rightRatchetUnlockPos; + public boolean ratchetsUnlocked() { + return rightRatchetServo.get() == LiftConstants.rightRatchetUnlockPos + && leftRatchetServo.get() == LiftConstants.leftRatchetUnlockPos; } - /** - * Runs the left lift motor up at a constant speed. - */ - public void runLeftUp() { - if (this.leftRatchetUnlocked()) { - if (isLiftUp()) { - leftLiftMotor.set(LiftConstants.motorSpeed); - } else { - leftLiftMotor.stopMotor(); - } - } else { - leftLiftMotor.stopMotor(); - } + public double getPosition() { + return leftLiftEncoder.getPosition(); } - /** - * Runs the right lift motor up at a constant speed. - */ - public void runRightUp() { - if (this.rightRatchetUnlocked()) { + public void runLiftUp() { + if (this.ratchetsUnlocked()) { if ((isLiftUp())) { - rightLiftMotor.set(LiftConstants.motorSpeed); + leftLiftMotor.set(LiftConstants.motorSpeed); } else { - rightLiftMotor.stopMotor(); + stopMotors(); } } else { - rightLiftMotor.stopMotor(); + stopMotors(); } } @@ -144,34 +107,21 @@ public boolean isLiftUp() { return rightLiftEncoder.getPosition() < LiftConstants.maxExtension; } - /** - * Runs the left lift motor down at a constant speed. - */ - public void runLeftDown() { - leftLiftMotor.set(LiftConstants.backwardsMotorSpeed); + public boolean onTarget() { + return getController().atSetpoint(); } - /** - * Runs the right lift motor down at a constant speed. - */ - public void runRightDown() { - rightLiftMotor.set(LiftConstants.backwardsMotorSpeed); + public void runLiftDown() { + leftLiftMotor.set(LiftConstants.backwardsMotorSpeed); } /** * Stops left lift motor. */ - public void stopLeftMotor() { + public void stopMotors() { leftLiftMotor.stopMotor(); } - /** - * Stops right lift motor. - */ - public void stopRightMotor() { - rightLiftMotor.stopMotor(); - } - /** * Determines if left limit switch is pressed * @@ -199,4 +149,15 @@ public void periodic() { rightLiftEncoder.setPosition(0); } } + + @Override + protected double getMeasurement() { + return getPosition(); + } + + @Override + protected void useOutput(double output, double setpoint) { + double power = MathUtil.clamp(output, -LiftConstants.maxPower, LiftConstants.maxPower); + leftLiftMotor.set(power); + } } From 0be85b7853e02913b1fb049faa8f365de63a91f4 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:57:16 -0400 Subject: [PATCH 06/18] made encoder conversion for lift pID --- src/main/java/frc/robot/constants/LiftConstants.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index 1158ee3f..dbe0f7a4 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -16,6 +16,7 @@ public class LiftConstants { public static final double backwardsMotorSpeed = -0.9; public static final double maxExtension = 195; + public static final double maxLiftInches = 27; public static final double maxPower = 1.0; @@ -24,5 +25,5 @@ public class LiftConstants { public static final double kD = 0.0; public static final double tolerance = 2.0; - public static final double encoderConversion = 0.0; + public static final double encoderConversion = 1 / (maxExtension / maxLiftInches); } \ No newline at end of file From 889014f083889c540f2526f1b3152a6b52c76092 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 11:59:10 -0400 Subject: [PATCH 07/18] made set p i and d functions for lift --- src/main/java/frc/robot/subsystems/Lift.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index 80534a23..41ee7661 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -160,4 +160,16 @@ protected void useOutput(double output, double setpoint) { double power = MathUtil.clamp(output, -LiftConstants.maxPower, LiftConstants.maxPower); leftLiftMotor.set(power); } + + public void setP(double p) { + getController().setP(p); + } + + public void setI(double i) { + getController().setI(i); + } + + public void setD(double d) { + getController().setD(d); + } } From 899a25364b6fb2bbcdbe407d3f3816a328b9fb53 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 13:29:38 -0400 Subject: [PATCH 08/18] added tolerance for lift PID --- src/main/java/frc/robot/constants/LiftConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index dbe0f7a4..bf7a5bac 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -24,6 +24,6 @@ public class LiftConstants { public static final double kI = 0.0; public static final double kD = 0.0; - public static final double tolerance = 2.0; + public static final double tolerance = 1.0; public static final double encoderConversion = 1 / (maxExtension / maxLiftInches); } \ No newline at end of file From fd189c3cb668d1124c6fd852454875e85014c262 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 14:51:03 -0400 Subject: [PATCH 09/18] fixed the PID for lift subsystem to be good: --- .../frc/robot/commands/LiftDownCommand.java | 6 +- .../frc/robot/commands/LiftUpCommand.java | 6 +- .../frc/robot/constants/LiftConstants.java | 11 +- .../java/frc/robot/subsystems/DriveTrain.java | 9 + src/main/java/frc/robot/subsystems/Lift.java | 224 ++++++++++++++---- 5 files changed, 205 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftDownCommand.java b/src/main/java/frc/robot/commands/LiftDownCommand.java index 6d1489da..6c360632 100644 --- a/src/main/java/frc/robot/commands/LiftDownCommand.java +++ b/src/main/java/frc/robot/commands/LiftDownCommand.java @@ -1,6 +1,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.LiftConstants; import frc.robot.subsystems.Lift; public class LiftDownCommand extends Command { @@ -12,7 +13,8 @@ public LiftDownCommand() { @Override public void execute() { - lift.runLiftDown(); + lift.enable(); + lift.setSetpoint(LiftConstants.minLiftInches); } @Override @@ -22,6 +24,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - lift.stopMotors(); + lift.disable(); } } diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index fb6833c6..e0498852 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -1,6 +1,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.LiftConstants; import frc.robot.subsystems.Lift; public class LiftUpCommand extends Command { @@ -17,7 +18,8 @@ public void initialize() { @Override public void execute() { - lift.runLiftUp(); + lift.enable(); + lift.setSetpoint(LiftConstants.maxLiftInches); } @Override @@ -27,7 +29,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - lift.stopMotors(); lift.enableRatchets(); + lift.disable(); } } diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index bf7a5bac..7e122994 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -17,12 +17,17 @@ public class LiftConstants { public static final double maxExtension = 195; public static final double maxLiftInches = 27; + public static final double minLiftInches = 0; public static final double maxPower = 1.0; - public static final double kP = 0.01; - public static final double kI = 0.0; - public static final double kD = 0.0; + public static final double kVerticalP = 0.01; + public static final double kVerticalI = 0.0; + public static final double kVerticalD = 0.0; + + public static final double kErrorP = 0.01; + public static final double kErrorI = 0.0; + public static final double kErrorD = 0.0; public static final double tolerance = 1.0; public static final double encoderConversion = 1 / (maxExtension / maxLiftInches); diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 8dccb730..8cf840ec 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -263,4 +263,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(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index 41ee7661..f44d8202 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -9,11 +9,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.LiftConstants; import frc.robot.constants.NeoMotorConstants; -public final class Lift extends PIDSubsystem { +public final class Lift extends SubsystemBase { private CANSparkMax leftLiftMotor = new CANSparkMax(LiftConstants.leftMotorPort, MotorType.kBrushless); private CANSparkMax rightLiftMotor = new CANSparkMax(LiftConstants.rightMotorPort, MotorType.kBrushless); private RelativeEncoder leftLiftEncoder = leftLiftMotor.getEncoder(); @@ -25,6 +25,17 @@ public final class Lift extends PIDSubsystem { private Servo leftRatchetServo = new Servo(LiftConstants.leftServoPort); private Servo rightRatchetServo = new Servo(LiftConstants.rightServoPort); + private PIDController verticalController = new PIDController( + LiftConstants.kVerticalP, + LiftConstants.kVerticalI, + LiftConstants.kVerticalD); + private PIDController errorController = new PIDController( + LiftConstants.kErrorP, + LiftConstants.kErrorI, + LiftConstants.kErrorD); + + private boolean enabled; + private static Lift instance; /** @@ -44,9 +55,6 @@ public static Lift getInstance() { * follow the left. Burns these settings to the flash of each motor. */ private Lift() { - super(new PIDController(LiftConstants.kP, LiftConstants.kI, LiftConstants.kD)); - getController().setTolerance(LiftConstants.tolerance); - getController().enableContinuousInput(0, LiftConstants.encoderConversion); leftLiftMotor.restoreFactoryDefaults(); rightLiftMotor.restoreFactoryDefaults(); @@ -56,8 +64,6 @@ private Lift() { leftLimitSwitch.enableLimitSwitch(true); rightLimitSwitch.enableLimitSwitch(true); - rightLiftMotor.follow(leftLiftMotor); - leftLiftMotor.setInverted(false); rightLiftMotor.setInverted(true); @@ -69,57 +75,91 @@ private Lift() { leftLiftMotor.burnFlash(); rightLiftMotor.burnFlash(); + + verticalController.disableContinuousInput(); + verticalController.setTolerance(LiftConstants.tolerance); + + errorController.disableContinuousInput(); + errorController.setTolerance(LiftConstants.tolerance); + errorController.setSetpoint(0); } + /** + * Enables the lift ratchets (sets them to a constant maximum extension). + */ public void enableRatchets() { - rightRatchetServo.set(LiftConstants.rightRatchetLockPos); leftRatchetServo.set(LiftConstants.leftRatchetLockPos); + rightRatchetServo.set(LiftConstants.rightRatchetLockPos); } + /** + * Disables the lift ratchets (sets them to a constant minimum extension). + */ public void disableRatchets() { leftRatchetServo.set(LiftConstants.leftRatchetUnlockPos); rightRatchetServo.set(LiftConstants.rightRatchetUnlockPos); - } - public boolean ratchetsUnlocked() { - return rightRatchetServo.get() == LiftConstants.rightRatchetUnlockPos - && leftRatchetServo.get() == LiftConstants.leftRatchetUnlockPos; + /** + * Determines if the left ratchet is in the unlocked position + * + * @return is the ratchet unlocked + */ + public boolean leftRatchetUnlocked() { + return leftRatchetServo.get() == LiftConstants.leftRatchetUnlockPos; } - public double getPosition() { - return leftLiftEncoder.getPosition(); + /** + * Determines if the right ratchet is in the unlocked position + * + * @return is the ratchet unlocked + */ + public boolean rightRatchetUnlocked() { + return rightRatchetServo.get() == LiftConstants.rightRatchetUnlockPos; } - public void runLiftUp() { - if (this.ratchetsUnlocked()) { - if ((isLiftUp())) { - leftLiftMotor.set(LiftConstants.motorSpeed); + /** + * Runs the left lift motor up at a constant speed. + */ + private void runLeftUp(double speed) { + if (this.leftRatchetUnlocked()) { + if (isLiftUp()) { + leftLiftMotor.set(speed); } else { - stopMotors(); + leftLiftMotor.stopMotor(); } } else { - stopMotors(); + leftLiftMotor.stopMotor(); } } - public boolean isLiftUp() { - return rightLiftEncoder.getPosition() < LiftConstants.maxExtension; - } - - public boolean onTarget() { - return getController().atSetpoint(); + /** + * Runs the right lift motor up at a constant speed. + */ + private void runRightUp(double speed) { + if (this.rightRatchetUnlocked()) { + if ((isLiftUp())) { + rightLiftMotor.set(speed); + } else { + rightLiftMotor.stopMotor(); + } + } else { + rightLiftMotor.stopMotor(); + } } - public void runLiftDown() { + /** + * Runs the left lift motor down at a constant speed. + */ + public void runLeftDown() { leftLiftMotor.set(LiftConstants.backwardsMotorSpeed); } /** - * Stops left lift motor. + * Runs the right lift motor down at a constant speed. */ - public void stopMotors() { - leftLiftMotor.stopMotor(); + public void runRightDown() { + rightLiftMotor.set(LiftConstants.backwardsMotorSpeed); } /** @@ -142,34 +182,130 @@ public boolean rightLowerLimitReached() { @Override public void periodic() { + if (enabled) { + double output = verticalController.calculate(getLeftPosition()); + double error = errorController.calculate(DriveTrain.getInstance().getPitch()); + + double power = MathUtil.clamp(output, LiftConstants.backwardsMotorSpeed, LiftConstants.motorSpeed); + double clampedError = MathUtil.clamp(error, LiftConstants.backwardsMotorSpeed, LiftConstants.motorSpeed); + + double left = DriveTrain.getInstance().getPitch() > 0 ? power + clampedError : power; + double right = DriveTrain.getInstance().getPitch() < 0 ? power - clampedError : power; + runLeftUp(left); + runRightUp(right); + } if (leftLimitSwitch.isPressed() && leftLiftEncoder.getPosition() != 0) { leftLiftEncoder.setPosition(0); } if (rightLimitSwitch.isPressed() && rightLiftEncoder.getPosition() != 0) { rightLiftEncoder.setPosition(0); } + } - @Override - protected double getMeasurement() { - return getPosition(); + /** + * Returns the vertical PIDController. + * + * @return The vertical controller. + */ + public PIDController getVerticalController() { + return verticalController; } - @Override - protected void useOutput(double output, double setpoint) { - double power = MathUtil.clamp(output, -LiftConstants.maxPower, LiftConstants.maxPower); - leftLiftMotor.set(power); + /** + * Returns the Right PIDController. + * + * @return The right controller. + */ + public PIDController getErrorController() { + return errorController; } - public void setP(double p) { - getController().setP(p); + /** + * Sets the Scoring PID setpoint to one of the constant setpoints. + * + * @param ElevatorSetpoints - desired setpoint + */ + public void setSetpoint(double setpoint) { + verticalController.setSetpoint(MathUtil.clamp(setpoint, 0, LiftConstants.maxExtension)); } - public void setI(double i) { - getController().setI(i); + /** + * Returns the current setpoint of the subsystem. + * + * @return The current setpoint + */ + public double getSetpoint() { + return verticalController.getSetpoint(); + } + + /** + * Returns the position of the left lift encoder. + * + * @return double - current encoder position + */ + public double getLeftPosition() { + return leftLiftEncoder.getPosition(); } - public void setD(double d) { - getController().setD(d); + /** + * Returns the position of the right lift encoder. + * + * @return double - current encoder position + */ + public double getRightPosition() { + return rightLiftEncoder.getPosition(); + } + + public boolean isLiftUp() { + return rightLiftEncoder.getPosition() < LiftConstants.maxExtension; + } + + /** + * Stops left lift motor. + */ + public void stopLeftMotor() { + leftLiftMotor.stopMotor(); + } + + /** + * Stops right lift motor. + */ + public void stopRightMotor() { + rightLiftMotor.stopMotor(); + } + + /** + * Stops left lift motor. + */ + public void stopMotors() { + leftLiftMotor.stopMotor(); + rightLiftMotor.stopMotor(); + } + + /** Enables the PID control. Resets the controller. */ + public void enable() { + enabled = true; + verticalController.reset(); + errorController.reset(); + } + + /** Disables the PID control. Sets output to zero. */ + public void disable() { + enabled = false; + this.stopMotors(); + } + + /** + * Returns whether the controller is enabled. + * + * @return Whether the controller is enabled. + */ + public boolean isEnabled() { + return enabled; + } + + public boolean onTarget() { + return getVerticalController().atSetpoint(); } -} +} \ No newline at end of file From 1ac16f312070d125f8b760354bd2e5d9ee96f614 Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 16 Mar 2024 15:58:45 -0400 Subject: [PATCH 10/18] started fixing changes wes asked for --- src/main/java/frc/robot/DriverJoystick.java | 4 +- .../frc/robot/commands/LiftDownCommand.java | 6 ++- .../frc/robot/commands/LiftUpCommand.java | 2 +- .../frc/robot/constants/LiftConstants.java | 4 +- src/main/java/frc/robot/subsystems/Lift.java | 39 ++++++++++++------- 5 files changed, 34 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index 6b6ad3ca..767df9d5 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -91,8 +91,8 @@ private DriverJoystick() { .onTrue(new InstantCommand(vision::enable0, vision)) .onFalse(new InstantCommand(vision::disable0, vision)); - rightBumper.and(leftBumper.negate()).whileTrue(new LiftUpCommand()); - rightTrigger.and(leftTrigger.negate()).whileTrue(new LiftDownCommand()); + rightBumper.whileTrue(new LiftUpCommand()); + rightTrigger.whileTrue(new LiftDownCommand()); leftBumper.and(rightBumper).whileTrue(new LiftUpCommand()); leftTrigger.and(rightTrigger).whileTrue(new LiftDownCommand()); diff --git a/src/main/java/frc/robot/commands/LiftDownCommand.java b/src/main/java/frc/robot/commands/LiftDownCommand.java index 6c360632..f9ef050a 100644 --- a/src/main/java/frc/robot/commands/LiftDownCommand.java +++ b/src/main/java/frc/robot/commands/LiftDownCommand.java @@ -12,8 +12,12 @@ public LiftDownCommand() { } @Override - public void execute() { + public void initialize() { lift.enable(); + } + + @Override + public void execute() { lift.setSetpoint(LiftConstants.minLiftInches); } diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index e0498852..18ed4df1 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -14,11 +14,11 @@ public LiftUpCommand() { @Override public void initialize() { lift.disableRatchets(); + lift.enable(); } @Override public void execute() { - lift.enable(); lift.setSetpoint(LiftConstants.maxLiftInches); } diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index 7e122994..035dab23 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -12,8 +12,8 @@ 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 maxLiftInches = 27; diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index f44d8202..1d2c6801 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -14,6 +14,8 @@ import frc.robot.constants.NeoMotorConstants; public final class Lift extends SubsystemBase { + private DriveTrain driveTrain = DriveTrain.getInstance(); + private CANSparkMax leftLiftMotor = new CANSparkMax(LiftConstants.leftMotorPort, MotorType.kBrushless); private CANSparkMax rightLiftMotor = new CANSparkMax(LiftConstants.rightMotorPort, MotorType.kBrushless); private RelativeEncoder leftLiftEncoder = leftLiftMotor.getEncoder(); @@ -121,13 +123,16 @@ public boolean rightRatchetUnlocked() { /** * Runs the left lift motor up at a constant speed. */ - private void runLeftUp(double speed) { - if (this.leftRatchetUnlocked()) { - if (isLiftUp()) { + private void runLeft(double speed) { + speed = MathUtil.clamp(speed, LiftConstants.downSpeed, LiftConstants.upSpeed); + if (speed > 0 && this.leftRatchetUnlocked()) { + if (!isLiftUp()) { leftLiftMotor.set(speed); } else { leftLiftMotor.stopMotor(); } + } else if (speed < 0) { + leftLiftMotor.set(speed); } else { leftLiftMotor.stopMotor(); } @@ -136,13 +141,16 @@ private void runLeftUp(double speed) { /** * Runs the right lift motor up at a constant speed. */ - private void runRightUp(double speed) { - if (this.rightRatchetUnlocked()) { - if ((isLiftUp())) { + private void runRight(double speed) { + speed = MathUtil.clamp(speed, LiftConstants.downSpeed, LiftConstants.upSpeed); + if (speed > 0 && this.rightRatchetUnlocked()) { + if ((!isLiftUp())) { rightLiftMotor.set(speed); } else { rightLiftMotor.stopMotor(); } + } else if (speed < 0) { + rightLiftMotor.set(speed); } else { rightLiftMotor.stopMotor(); } @@ -152,14 +160,14 @@ private void runRightUp(double speed) { * Runs the left lift motor down at a constant speed. */ public void runLeftDown() { - leftLiftMotor.set(LiftConstants.backwardsMotorSpeed); + leftLiftMotor.set(LiftConstants.downSpeed); } /** * Runs the right lift motor down at a constant speed. */ public void runRightDown() { - rightLiftMotor.set(LiftConstants.backwardsMotorSpeed); + rightLiftMotor.set(LiftConstants.downSpeed); } /** @@ -184,15 +192,16 @@ public boolean rightLowerLimitReached() { public void periodic() { if (enabled) { double output = verticalController.calculate(getLeftPosition()); - double error = errorController.calculate(DriveTrain.getInstance().getPitch()); + double pitch = driveTrain.getPitch(); + double error = errorController.calculate(pitch); - double power = MathUtil.clamp(output, LiftConstants.backwardsMotorSpeed, LiftConstants.motorSpeed); - double clampedError = MathUtil.clamp(error, LiftConstants.backwardsMotorSpeed, LiftConstants.motorSpeed); + double power = MathUtil.clamp(output, LiftConstants.downSpeed, LiftConstants.upSpeed); + double clampedError = MathUtil.clamp(error, LiftConstants.downSpeed, LiftConstants.upSpeed); - double left = DriveTrain.getInstance().getPitch() > 0 ? power + clampedError : power; - double right = DriveTrain.getInstance().getPitch() < 0 ? power - clampedError : power; - runLeftUp(left); - runRightUp(right); + double left = pitch > 0 ? power + clampedError : power; + double right = pitch < 0 ? power - clampedError : power; + runLeft(left); + runRight(right); } if (leftLimitSwitch.isPressed() && leftLiftEncoder.getPosition() != 0) { leftLiftEncoder.setPosition(0); From f6853faf62f8f80eff6eab6598e0cf3058910ba4 Mon Sep 17 00:00:00 2001 From: Griffin Date: Mon, 18 Mar 2024 16:55:55 -0400 Subject: [PATCH 11/18] fixed changes for lift that wes sugguested --- .../frc/robot/constants/LiftConstants.java | 1 + src/main/java/frc/robot/subsystems/Lift.java | 24 ++++++++++++++----- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index 035dab23..988a1f86 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -16,6 +16,7 @@ public class LiftConstants { 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; diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index 8403629b..d5dec12c 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -127,8 +127,8 @@ 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) { - leftLiftMotor.set(LiftConstants.motorSpeed); + if (!(leftLiftEncoder.getPosition() < LiftConstants.maxExtension)) { + leftLiftMotor.set(LiftConstants.maxExtension); } else { leftLiftMotor.stopMotor(); } @@ -145,7 +145,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 ((!isLiftUp())) { + if ((!(rightLiftEncoder.getPosition() < LiftConstants.maxExtension))) { rightLiftMotor.set(speed); } else { rightLiftMotor.stopMotor(); @@ -237,7 +237,8 @@ public PIDController getErrorController() { * @param ElevatorSetpoints - desired setpoint */ public void setSetpoint(double setpoint) { - verticalController.setSetpoint(MathUtil.clamp(setpoint, 0, LiftConstants.maxExtension)); + verticalController + .setSetpoint(MathUtil.clamp(setpoint, LiftConstants.minExtension, LiftConstants.maxExtension)); } /** @@ -252,7 +253,7 @@ public double getSetpoint() { /** * Returns the position of the left lift encoder. * - * @return double - current encoder position + * @return current encoder position */ public double getLeftPosition() { return leftLiftEncoder.getPosition(); @@ -267,8 +268,14 @@ public double getRightPosition() { return rightLiftEncoder.getPosition(); } + /** + * Returns whether or not lift is up + * + * @return whether lift is up or not + */ public boolean isLiftUp() { - return rightLiftEncoder.getPosition() < LiftConstants.maxExtension; + return rightLiftEncoder.getPosition() < LiftConstants.maxExtension + && leftLiftEncoder.getPosition() < LiftConstants.maxExtension; } /** @@ -315,6 +322,11 @@ public boolean isEnabled() { return enabled; } + /** + * Returns whether lift is at setpoint. + * + * @return whether lift is at setpoint + */ public boolean onTarget() { return getVerticalController().atSetpoint(); } From 3632c90390ad519cfd0d9e7b72e84784e5b27118 Mon Sep 17 00:00:00 2001 From: Griffin Date: Tue, 19 Mar 2024 16:42:35 -0400 Subject: [PATCH 12/18] deleteed unused import --- src/main/java/frc/robot/subsystems/Lift.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Lift.java b/src/main/java/frc/robot/subsystems/Lift.java index d5dec12c..f8216c2c 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.LiftDownCommand; import frc.robot.constants.LiftConstants; import frc.robot.constants.NeoMotorConstants; From 80177ce2528e7bbab37b30468ef285d01f85e4dc Mon Sep 17 00:00:00 2001 From: Griffin Date: Thu, 21 Mar 2024 17:20:08 -0400 Subject: [PATCH 13/18] removed dashboqrd graph --- src/main/java/frc/robot/subsystems/Dashboard.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index fb4d9f0d..7cd46bf1 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -10,11 +10,9 @@ 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; -import edu.wpi.first.wpilibj.shuffleboard.WidgetType; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -34,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 @@ -103,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); From bc2c99aaa8a635f47266cafe372fe71ab29ca3c9 Mon Sep 17 00:00:00 2001 From: Griffin Date: Thu, 21 Mar 2024 17:46:57 -0400 Subject: [PATCH 14/18] lift runs with new code (WIP) --- src/main/java/frc/robot/DriverJoystick.java | 4 +-- src/main/java/frc/robot/Robot.java | 9 +++--- .../robot/commands/LiftDownManualCommand.java | 28 +++++++++++++++++++ .../java/frc/robot/subsystems/Dashboard.java | 7 +++++ src/main/java/frc/robot/subsystems/Lift.java | 12 +++++--- 5 files changed, 49 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/commands/LiftDownManualCommand.java diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index 767df9d5..a39e7633 100644 --- 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 index f3d72887..e0377bfb 100644 --- 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 new file mode 100644 index 00000000..a1c25e97 --- /dev/null +++ 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 index 7cd46bf1..8c40c5db 100644 --- 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 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 index f8216c2c..ad2ff568 100644 --- 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; } /** From 711d9d99eecce12139687edd7c08575f7271373e Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 23 Mar 2024 09:46:23 -0400 Subject: [PATCH 15/18] made a third PID controller for the lift --- .../frc/robot/commands/LiftDownCommand.java | 3 +- .../frc/robot/commands/LiftUpCommand.java | 3 +- .../frc/robot/constants/LiftConstants.java | 10 +- .../java/frc/robot/subsystems/Dashboard.java | 6 +- src/main/java/frc/robot/subsystems/Lift.java | 98 ++++++++++++++----- 5 files changed, 87 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftDownCommand.java b/src/main/java/frc/robot/commands/LiftDownCommand.java index f9ef050a..469bb0cf 100644 --- a/src/main/java/frc/robot/commands/LiftDownCommand.java +++ b/src/main/java/frc/robot/commands/LiftDownCommand.java @@ -18,7 +18,8 @@ public void initialize() { @Override public void execute() { - lift.setSetpoint(LiftConstants.minLiftInches); + lift.setSetpointLeft(LiftConstants.minLiftInches); + lift.setSetpointRight(LiftConstants.minLiftInches); } @Override diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index f3b4eb07..3ff5c398 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -19,7 +19,8 @@ public void initialize() { @Override public void execute() { - lift.setSetpoint(LiftConstants.maxLiftInches); + lift.setSetpointLeft(LiftConstants.maxLiftInches); + lift.setSetpointRight(LiftConstants.maxLiftInches); } @Override diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index 988a1f86..90323d39 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -22,9 +22,13 @@ public class LiftConstants { public static final double maxPower = 1.0; - public static final double kVerticalP = 0.01; - public static final double kVerticalI = 0.0; - public static final double kVerticalD = 0.0; + public static final double kVerticalLeftP = 0.01; + public static final double kVerticalLeftI = 0.0; + public static final double kVerticalLeftD = 0.0; + + public static final double kVerticalRightP = 0.01; + public static final double kVerticalRightI = 0.0; + public static final double kVerticalRightD = 0.0; public static final double kErrorP = 0.01; public static final double kErrorI = 0.0; diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index 8c40c5db..8c69ea39 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -103,10 +103,12 @@ private Dashboard() { .withSize(4, 3) .withWidget(BuiltInWidgets.kField); - tempTab.add("Vert", lift.getVerticalController()) + tempTab.add("Vert-Left", lift.getLeftVerticalController()) .withPosition(0, 0); - tempTab.add("Err", lift.getErrorController()) + tempTab.add("Vert-Right", lift.getRightVerticalController()) .withPosition(1, 0); + tempTab.add("Err", lift.getErrorController()) + .withPosition(2, 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 index ad2ff568..10b2db2b 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -27,10 +27,14 @@ public final class Lift extends SubsystemBase { private Servo leftRatchetServo = new Servo(LiftConstants.leftServoPort); private Servo rightRatchetServo = new Servo(LiftConstants.rightServoPort); - private PIDController verticalController = new PIDController( - LiftConstants.kVerticalP, - LiftConstants.kVerticalI, - LiftConstants.kVerticalD); + private PIDController verticalControllerLeft = new PIDController( + LiftConstants.kVerticalLeftP, + LiftConstants.kVerticalLeftI, + LiftConstants.kVerticalLeftD); + private PIDController verticalControllerRight = new PIDController( + LiftConstants.kVerticalRightP, + LiftConstants.kVerticalRightI, + LiftConstants.kVerticalRightD); private PIDController errorController = new PIDController( LiftConstants.kErrorP, LiftConstants.kErrorI, @@ -81,8 +85,10 @@ private Lift() { leftLiftMotor.burnFlash(); rightLiftMotor.burnFlash(); - verticalController.disableContinuousInput(); - verticalController.setTolerance(LiftConstants.tolerance); + verticalControllerLeft.disableContinuousInput(); + verticalControllerLeft.setTolerance(LiftConstants.tolerance); + verticalControllerRight.disableContinuousInput(); + verticalControllerRight.setTolerance(LiftConstants.tolerance); errorController.disableContinuousInput(); errorController.setTolerance(LiftConstants.tolerance); @@ -194,15 +200,17 @@ public boolean rightLowerLimitReached() { @Override public void periodic() { if (enabled) { - double output = verticalController.calculate(getLeftPosition()); + double leftOutput = verticalControllerLeft.calculate(getLeftPosition()); + double rightOutput = verticalControllerRight.calculate(getRightPosition()); double pitch = driveTrain.getPitch(); double error = errorController.calculate(pitch); - double power = MathUtil.clamp(output, LiftConstants.downSpeed, LiftConstants.upSpeed); + double leftPower = MathUtil.clamp(leftOutput, LiftConstants.downSpeed, LiftConstants.upSpeed); + double rightPower = MathUtil.clamp(rightOutput, LiftConstants.downSpeed, LiftConstants.upSpeed); double clampedError = MathUtil.clamp(error, LiftConstants.downSpeed, LiftConstants.upSpeed); - double left = pitch > 0 ? power + clampedError : power; - double right = pitch < 0 ? power - clampedError : power; + double left = pitch > 0 ? leftPower + clampedError : leftPower; + double right = pitch < 0 ? rightPower - clampedError : rightPower; runLeft(left); runRight(right); @@ -217,12 +225,21 @@ public void periodic() { } /** - * Returns the vertical PIDController. + * Returns the Left vertical PIDController. * - * @return The vertical controller. + * @return The Left vertical controller. */ - public PIDController getVerticalController() { - return verticalController; + public PIDController getLeftVerticalController() { + return verticalControllerLeft; + } + + /** + * Returns the RIght vertical PIDController. + * + * @return The Right vertical controller. + */ + public PIDController getRightVerticalController() { + return verticalControllerRight; } /** @@ -235,22 +252,41 @@ public PIDController getErrorController() { } /** - * Sets the Scoring PID setpoint to one of the constant setpoints. + * Sets the Left Lift PID setpoint to one of the constant setpoints. + * + * @param Double - desired setpoint + */ + public void setSetpointLeft(double setpoint) { + verticalControllerLeft + .setSetpoint(MathUtil.clamp(setpoint, LiftConstants.minExtension, LiftConstants.maxExtension)); + } + + /** + * Sets the Right Lift PID setpoint to one of the constant setpoints. * - * @param ElevatorSetpoints - desired setpoint + * @param Double - desired setpoint */ - public void setSetpoint(double setpoint) { - verticalController + public void setSetpointRight(double setpoint) { + verticalControllerRight .setSetpoint(MathUtil.clamp(setpoint, LiftConstants.minExtension, LiftConstants.maxExtension)); } /** * Returns the current setpoint of the subsystem. * - * @return The current setpoint + * @return The current setpoint of the Left Lift */ - public double getSetpoint() { - return verticalController.getSetpoint(); + public double getLeftSetpoint() { + return verticalControllerLeft.getSetpoint(); + } + + /** + * Returns the current setpoint of the subsystem. + * + * @return The current setpoint of the Right Lift + */ + public double getRightSetpoint() { + return verticalControllerRight.getSetpoint(); } /** @@ -306,7 +342,8 @@ public void stopMotors() { /** Enables the PID control. Resets the controller. */ public void enable() { enabled = true; - verticalController.reset(); + verticalControllerLeft.reset(); + verticalControllerRight.reset(); errorController.reset(); } @@ -326,11 +363,20 @@ public boolean isEnabled() { } /** - * Returns whether lift is at setpoint. + * Returns whether left lift is at setpoint. + * + * @return whether left lift is at setpoint + */ + public boolean leftOnTarget() { + return getLeftVerticalController().atSetpoint(); + } + + /** + * Returns whether right lift is at setpoint. * - * @return whether lift is at setpoint + * @return whether right lift is at setpoint */ - public boolean onTarget() { - return getVerticalController().atSetpoint(); + public boolean rightOnTarget() { + return getRightVerticalController().atSetpoint(); } } \ No newline at end of file From 438fd2743aa3b38a019165c1f4cc02d87d94bd5f Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 23 Mar 2024 14:48:58 -0400 Subject: [PATCH 16/18] tuned PID for lift --- .../frc/robot/commands/LiftDownCommand.java | 13 ++-- .../frc/robot/commands/LiftUpCommand.java | 15 +++-- .../frc/robot/constants/LiftConstants.java | 20 +++--- .../java/frc/robot/subsystems/Dashboard.java | 10 +++ src/main/java/frc/robot/subsystems/Lift.java | 61 +++++++++---------- 5 files changed, 69 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftDownCommand.java b/src/main/java/frc/robot/commands/LiftDownCommand.java index 469bb0cf..dc3e40fa 100644 --- a/src/main/java/frc/robot/commands/LiftDownCommand.java +++ b/src/main/java/frc/robot/commands/LiftDownCommand.java @@ -1,5 +1,6 @@ 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; @@ -13,11 +14,15 @@ public LiftDownCommand() { @Override 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(); - } - - @Override - public void execute() { lift.setSetpointLeft(LiftConstants.minLiftInches); lift.setSetpointRight(LiftConstants.minLiftInches); } diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java index 3ff5c398..2c57bbba 100644 --- a/src/main/java/frc/robot/commands/LiftUpCommand.java +++ b/src/main/java/frc/robot/commands/LiftUpCommand.java @@ -1,5 +1,6 @@ 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; @@ -14,12 +15,16 @@ public LiftUpCommand() { @Override public void initialize() { 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(); - } - - @Override - public void execute() { - lift.setSetpointLeft(LiftConstants.maxLiftInches); + lift.setSetpointLeft(LiftConstants.maxLiftInches - 0.5); lift.setSetpointRight(LiftConstants.maxLiftInches); } diff --git a/src/main/java/frc/robot/constants/LiftConstants.java b/src/main/java/frc/robot/constants/LiftConstants.java index 90323d39..8ebb1e6a 100644 --- a/src/main/java/frc/robot/constants/LiftConstants.java +++ b/src/main/java/frc/robot/constants/LiftConstants.java @@ -22,18 +22,22 @@ public class LiftConstants { public static final double maxPower = 1.0; - public static final double kVerticalLeftP = 0.01; - public static final double kVerticalLeftI = 0.0; - public static final double kVerticalLeftD = 0.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 kVerticalRightP = 0.01; - public static final double kVerticalRightI = 0.0; - public static final double kVerticalRightD = 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 kErrorP = 0.01; + 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 = 1.0; + public static final double tolerance = 0; public static final double encoderConversion = 1 / (maxExtension / maxLiftInches); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index 8c69ea39..c5d3f986 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -109,6 +109,16 @@ private Dashboard() { .withPosition(1, 0); tempTab.add("Err", lift.getErrorController()) .withPosition(2, 0); + tempTab.addNumber("Left Pos", lift::getLeftPosition) + .withPosition(0, 2); + tempTab.addNumber("Right Pos", lift::getRightPosition) + .withPosition(1, 2); + tempTab.addNumber("Pitch", driveTrain::getRoll) + .withPosition(2, 2); + tempTab.addBoolean("Left lim", lift::leftLowerLimitReached) + .withPosition(0, 3); + tempTab.addBoolean("Right lim", lift::rightLowerLimitReached) + .withPosition(1, 3); 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 index 10b2db2b..25e592b2 100644 --- a/src/main/java/frc/robot/subsystems/Lift.java +++ b/src/main/java/frc/robot/subsystems/Lift.java @@ -2,6 +2,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; + +import java.util.function.BooleanSupplier; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkLimitSwitch; @@ -28,13 +31,13 @@ public final class Lift extends SubsystemBase { private Servo rightRatchetServo = new Servo(LiftConstants.rightServoPort); private PIDController verticalControllerLeft = new PIDController( - LiftConstants.kVerticalLeftP, - LiftConstants.kVerticalLeftI, - LiftConstants.kVerticalLeftD); + LiftConstants.kVerticalLeftUpP, + LiftConstants.kVerticalLeftUpI, + LiftConstants.kVerticalLeftUpD); private PIDController verticalControllerRight = new PIDController( - LiftConstants.kVerticalRightP, - LiftConstants.kVerticalRightI, - LiftConstants.kVerticalRightD); + LiftConstants.kVerticalRightUpP, + LiftConstants.kVerticalRightUpI, + LiftConstants.kVerticalRightUpD); private PIDController errorController = new PIDController( LiftConstants.kErrorP, LiftConstants.kErrorI, @@ -91,7 +94,6 @@ private Lift() { verticalControllerRight.setTolerance(LiftConstants.tolerance); errorController.disableContinuousInput(); - errorController.setTolerance(LiftConstants.tolerance); errorController.setSetpoint(0); } @@ -129,40 +131,33 @@ public boolean rightRatchetUnlocked() { return rightRatchetServo.get() == LiftConstants.rightRatchetUnlockPos; } - /** - * Runs the left lift motor up at a constant speed. - */ - private void runLeft(double speed) { + private void run(CANSparkMax motor, RelativeEncoder encoder, BooleanSupplier ratchetUnlocked, double speed) { speed = MathUtil.clamp(speed, LiftConstants.downSpeed, LiftConstants.upSpeed); - if (speed > 0 && this.leftRatchetUnlocked()) { - if (leftLiftEncoder.getPosition() < LiftConstants.maxExtension) { - leftLiftMotor.set(LiftConstants.maxExtension); + if (speed > 0 && ratchetUnlocked.getAsBoolean()) { + if (encoder.getPosition() < LiftConstants.maxExtension) { + motor.set(LiftConstants.maxExtension); } else { - leftLiftMotor.stopMotor(); + motor.stopMotor(); } } else if (speed < 0) { - leftLiftMotor.set(speed); + motor.set(speed); } else { - leftLiftMotor.stopMotor(); + motor.stopMotor(); } } + /** + * Runs the left lift motor up at a constant speed. + */ + private void runLeft(double speed) { + this.run(leftLiftMotor, leftLiftEncoder, this::leftRatchetUnlocked, speed); + } + /** * Runs the right lift motor up at a constant speed. */ private void runRight(double speed) { - speed = MathUtil.clamp(speed, LiftConstants.downSpeed, LiftConstants.upSpeed); - if (speed > 0 && this.rightRatchetUnlocked()) { - if (rightLiftEncoder.getPosition() < LiftConstants.maxExtension) { - rightLiftMotor.set(speed); - } else { - rightLiftMotor.stopMotor(); - } - } else if (speed < 0) { - rightLiftMotor.set(speed); - } else { - rightLiftMotor.stopMotor(); - } + this.run(rightLiftMotor, rightLiftEncoder, this::rightRatchetUnlocked, speed); } /** @@ -202,15 +197,15 @@ public void periodic() { if (enabled) { double leftOutput = verticalControllerLeft.calculate(getLeftPosition()); double rightOutput = verticalControllerRight.calculate(getRightPosition()); - double pitch = driveTrain.getPitch(); - double error = errorController.calculate(pitch); + double roll = driveTrain.getRoll(); + double error = errorController.calculate(roll); double leftPower = MathUtil.clamp(leftOutput, LiftConstants.downSpeed, LiftConstants.upSpeed); double rightPower = MathUtil.clamp(rightOutput, LiftConstants.downSpeed, LiftConstants.upSpeed); double clampedError = MathUtil.clamp(error, LiftConstants.downSpeed, LiftConstants.upSpeed); - double left = pitch > 0 ? leftPower + clampedError : leftPower; - double right = pitch < 0 ? rightPower - clampedError : rightPower; + double left = roll < 0 ? leftPower + clampedError : leftPower; + double right = roll > 0 ? rightPower - clampedError : rightPower; runLeft(left); runRight(right); From 1f51ad9a518e7419bb32e309bb19bfa4413c611d Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 23 Mar 2024 15:05:01 -0400 Subject: [PATCH 17/18] got rid of temp tab --- .../java/frc/robot/subsystems/Dashboard.java | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index c5d3f986..54d2bad1 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -33,7 +33,6 @@ 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 @@ -103,23 +102,6 @@ private Dashboard() { .withSize(4, 3) .withWidget(BuiltInWidgets.kField); - tempTab.add("Vert-Left", lift.getLeftVerticalController()) - .withPosition(0, 0); - tempTab.add("Vert-Right", lift.getRightVerticalController()) - .withPosition(1, 0); - tempTab.add("Err", lift.getErrorController()) - .withPosition(2, 0); - tempTab.addNumber("Left Pos", lift::getLeftPosition) - .withPosition(0, 2); - tempTab.addNumber("Right Pos", lift::getRightPosition) - .withPosition(1, 2); - tempTab.addNumber("Pitch", driveTrain::getRoll) - .withPosition(2, 2); - tempTab.addBoolean("Left lim", lift::leftLowerLimitReached) - .withPosition(0, 3); - tempTab.addBoolean("Right lim", lift::rightLowerLimitReached) - .withPosition(1, 3); - PathPlannerLogging.setLogTargetPoseCallback((pose) -> { field.getObject("target pose").setPose(pose); }); From 2e3f4f3ac2a79cf4e3423948a4aa8619a9d6d4ae Mon Sep 17 00:00:00 2001 From: Griffin Date: Sat, 23 Mar 2024 15:07:08 -0400 Subject: [PATCH 18/18] got rid of lift on dashboard --- src/main/java/frc/robot/subsystems/Dashboard.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index 54d2bad1..7cd46bf1 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -24,7 +24,6 @@ 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 autoChooser = new SendableChooser<>();