From 42d15ab1014bca245d76f8dc57150eb4e84b5914 Mon Sep 17 00:00:00 2001 From: Team 2648 Date: Mon, 17 Feb 2025 18:58:43 -0500 Subject: [PATCH] More work tuning the elevator --- src/main/java/frc/robot/RobotContainer.java | 10 +-- .../robot/constants/ElevatorConstants.java | 18 +++-- .../java/frc/robot/subsystems/Drivetrain.java | 5 +- .../java/frc/robot/subsystems/Elevator.java | 72 +++++++++++++++---- .../robot/subsystems/ManipulatorPivot.java | 9 +-- 5 files changed, 82 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 84c54d3..01d239c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,8 +56,8 @@ public class RobotContainer { climberRollers = new ClimberRollers(); - vision = new Vision(drivetrain::getGyroValue); - drivetrain = new Drivetrain(vision); + //vision = new Vision(drivetrain::getGyroValue); + drivetrain = new Drivetrain(); elevator = new Elevator(); //elevator = new ElevatorSysID(); @@ -153,13 +153,15 @@ public class RobotContainer { */ operator.povUp().onTrue( - elevator.goToSetpoint(() -> 20) + elevator.goToSetpoint(() -> 20).until(elevator::eitherAtGoal) ); operator.povDown().onTrue( - elevator.goToSetpoint(() -> 0) + elevator.goToSetpoint(() -> 0).until(elevator::eitherAtGoal) ); + operator.a().whileTrue(elevator.maintainPosition()); + /* operator.a().whileTrue(elevator.runManualElevator(() -> 0.2)); diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index a6aab0b..d3ad493 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -21,17 +21,21 @@ public class ElevatorConstants { public static final int kCurrentLimit = 40; - public static final double kPositionControllerP = 3; // - public static final double kPositionControllerI = 0; - public static final double kPositionControllerD = 0.1;//0.35 + public static final double kUpControllerP = 4;//7; // + public static final double kUpControllerI = 0; + public static final double kUpControllerD = 0.35;//0.1;//0.35 + + public static final double kDownControllerP = 6;//7; // + public static final double kDownControllerI = 0; + public static final double kDownControllerD = 0.175;//0.1;//0.35 public static final double kAllowedError = 0.75; - public static final double kFeedForwardS = 0; /* kG too high - kG too low / 2 */ - public static final double kFeedForwardG = .7; /* kG too high + kG too low / 2 */ // calculated value 0.6 - public static final double kFeedForwardV = 0.12; // calculated value 0.12 + public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */ + public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6 + public static final double kFeedForwardV = 0.06; // calculated value 0.12 - public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s + public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kCoralIntakePosition = 0; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a715ce1..3d0698a 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -44,7 +44,7 @@ public class Drivetrain extends SubsystemBase { private Vision vision; /** Creates a new DriveSubsystem. */ - public Drivetrain(Vision vision) { + public Drivetrain() { m_frontLeft = new MAXSwerveModule( DrivetrainConstants.kFrontLeftDrivingCanId, DrivetrainConstants.kFrontLeftTurningCanId, @@ -115,6 +115,7 @@ public class Drivetrain extends SubsystemBase { // if the detected tags match your alliances reef tags use their pose estimates + /* if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); @@ -127,7 +128,7 @@ public class Drivetrain extends SubsystemBase { }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); } - + */ } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index a6f22f8..316a1e5 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -25,7 +25,8 @@ public class Elevator extends SubsystemBase { private DigitalInput bottomLimitSwitch; - private ProfiledPIDController pidController; + private ProfiledPIDController pidControllerUp; + private ProfiledPIDController pidControllerDown; private ElevatorFeedforward feedForward; @@ -60,16 +61,28 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kBottomLimitSwitchID ); - pidController = new ProfiledPIDController( - ElevatorConstants.kPositionControllerP, - ElevatorConstants.kPositionControllerI, - ElevatorConstants.kPositionControllerD, + pidControllerUp = new ProfiledPIDController( + ElevatorConstants.kUpControllerP, + ElevatorConstants.kUpControllerI, + ElevatorConstants.kUpControllerD, new TrapezoidProfile.Constraints( ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration ) ); - pidController.setTolerance(ElevatorConstants.kAllowedError); + + pidControllerDown = new ProfiledPIDController( + ElevatorConstants.kDownControllerP, + ElevatorConstants.kDownControllerI, + ElevatorConstants.kDownControllerD, + new TrapezoidProfile.Constraints( + ElevatorConstants.kMaxVelocity, + ElevatorConstants.kMaxAcceleration + ) + ); + + pidControllerUp.setTolerance(ElevatorConstants.kAllowedError); + pidControllerDown.setTolerance(ElevatorConstants.kAllowedError); feedForward = new ElevatorFeedforward( ElevatorConstants.kFeedForwardS, @@ -114,9 +127,16 @@ public class Elevator extends SubsystemBase { */ public Command runManualElevator(DoubleSupplier speed) { return run(() -> { - elevatorMotor1.set( - speed.getAsDouble() - ); + double desired = speed.getAsDouble(); + + if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) { + elevatorMotor1.set( + speed.getAsDouble() + ); + } else { + elevatorMotor1.setVoltage(feedForward.calculate(0)); + } + }); } @@ -132,6 +152,10 @@ public class Elevator extends SubsystemBase { }); } + public boolean eitherAtGoal() { + return pidControllerUp.atGoal() || pidControllerDown.atGoal(); + } + /** * Moves the elevator to a target destination (setpoint). * @@ -147,17 +171,35 @@ public class Elevator extends SubsystemBase { ); return run(() -> { - pidController.reset(encoder.getPosition(), encoder.getVelocity()); - + //pidController.reset(encoder.getPosition(), encoder.getVelocity()); elevatorMotor1.setVoltage( - pidController.calculate( + pidControllerUp.calculate( encoder.getPosition(), clampedSetpoint - ) + feedForward.calculate(pidController.getSetpoint().velocity) + ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity) ); + }); } + + /* + if(encoder.getPosition() >= setpoint.getAsDouble()){ + elevatorMotor1.setVoltage( + pidControllerUp.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity) + ); + }else if(encoder.getPosition() <= setpoint.getAsDouble()){ + elevatorMotor1.setVoltage( + pidControllerDown.calculate( + encoder.getPosition(), + clampedSetpoint + ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity) + ); + } + */ /** * Returns the current encoder position @@ -184,7 +226,7 @@ public class Elevator extends SubsystemBase { * @return Motor output current */ public double getMotor1() { - return elevatorMotor1.getOutputCurrent(); + return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage(); } /** @@ -193,6 +235,6 @@ public class Elevator extends SubsystemBase { * @return Motor output current */ public double getMotor2() { - return elevatorMotor2.getOutputCurrent(); + return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index 2e89116..0fb39c4 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ManipulatorPivotConstants; @@ -40,10 +41,10 @@ public class ManipulatorPivot extends SubsystemBase { encoder = pivotMotor.getAbsoluteEncoder(); pidController = new ProfiledPIDController( - getEncoderPosition(), - getEncoderVelocity(), - getEncoderPosition(), - null + ManipulatorPivotConstants.kPositionalP, + ManipulatorPivotConstants.kPositionalI, + ManipulatorPivotConstants.kPositionalD, + new TrapezoidProfile.Constraints(ManipulatorPivotConstants.kMaxVelocity, ManipulatorPivotConstants.kMaxAcceleration) ); feedForward = new ArmFeedforward(