From 626b92b769f07f263799a8ab0230178774d6788e Mon Sep 17 00:00:00 2001 From: Tylr-J42 <84476781+Tylr-J42@users.noreply.github.com> Date: Mon, 19 May 2025 22:49:28 -0400 Subject: [PATCH] on controller elevator PID control --- src/main/java/frc/robot/RobotContainer.java | 20 ----- .../robot/constants/ElevatorConstants.java | 6 +- .../java/frc/robot/subsystems/Elevator.java | 79 ++++++++++--------- 3 files changed, 44 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 177a46f..8db8391 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -424,26 +424,6 @@ public class RobotContainer { sensorTab.addDouble("ElevMotor2", elevator::getMotor2) .withWidget(BuiltInWidgets.kGraph); - sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint) - .withSize(1, 1) - .withPosition(5, 0) - .withWidget(BuiltInWidgets.kTextView); - - sensorTab.addDouble("Elevator error up", elevator::getPIDUpError) - .withSize(1, 1) - .withPosition(5, 1) - .withWidget(BuiltInWidgets.kTextView); - - sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint) - .withSize(1, 1) - .withPosition(5, 0) - .withWidget(BuiltInWidgets.kTextView); - - sensorTab.addDouble("Elevator error down", elevator::getPIDDownError) - .withSize(1, 1) - .withPosition(5, 1) - .withWidget(BuiltInWidgets.kTextView); - sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput); sensorTab.addDouble("velocity", drivetrain::getVelocity); diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 559b9d2..5293f28 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -45,8 +45,10 @@ public class ElevatorConstants { 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.12; // calculated value 0.12 - public static final double kMaxVelocity = 100.0; // 120 inches per second (COOKING) calculated max is 184 in/s - public static final double kMaxAcceleration = 50; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 + public static final double kMaxVelocity = 100.0; // 100 inches per second (COOKING) calculated max is 184 in/s + public static final double kMaxAcceleration = 50; // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2 + public static final double kMaxVelocityAlgae = 120; + public static final double kMaxAccelerationAlgae = 400; public static final double kCoralIntakePosition = 0; public static final double kL1Position = 17; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 2f79d13..ca8eb8a 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -5,16 +5,17 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -28,16 +29,14 @@ public class Elevator extends SubsystemBase { private DigitalInput bottomLimitSwitch; - private PIDController pidController; - - private PIDController maintainPID; - private ElevatorFeedforward feedForward; private TrapezoidProfile trapProfile; + private TrapezoidProfile trapProfileAlgae; private TrapezoidProfile.State goal; - private TrapezoidProfile.State state; - private TrapezoidProfile.Constraints elevatorConstraints; + private TrapezoidProfile.State setpoint; + + private SparkClosedLoopController controller; public Elevator() { elevatorMotor1 = new SparkMax( @@ -74,7 +73,11 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kFeedForwardV ); - trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(elevatorConstraints.maxVelocity, ElevatorConstants.kMaxAcceleration)); + trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration)); + + trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae)); + + controller = elevatorMotor1.getClosedLoopController(); } @Override @@ -140,14 +143,12 @@ public class Elevator extends SubsystemBase { }, () -> { - - } - -/* - elevatorMotor1.setVoltage( - feedForward.calculate(0) - ); - */ + controller.setReference( + setpoint.position, + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedForward.calculate(0.0) + ); }); @@ -172,19 +173,34 @@ public class Elevator extends SubsystemBase { return startRun(() -> { goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0); }, () -> { - TrapezoidProfile.State setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal); + setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal); - - }).until(); + controller.setReference( + setpoint.position, + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedForward.calculate(encoder.getVelocity()) + ); + + }).until(() -> trapProfile.isFinished(encoder.getPosition())); } - public Command goToSetpointAlgae(DoubleSupplier setpoint) { - + public Command goToSetpointAlgae(DoubleSupplier setGoal) { + return startRun(() -> { + goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0); + }, () -> { + setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal); - - }).until(); + controller.setReference( + setpoint.position, + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + feedForward.calculate(setpoint.velocity) + ); + + }).until(() -> trapProfileAlgae.isFinished(encoder.getPosition())); } /** @@ -224,19 +240,4 @@ public class Elevator extends SubsystemBase { return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage(); } - public double getPIDUpSetpoint() { - return pidController.getSetpoint(); - } - - public double getPIDUpError() { - return pidController.getError(); - } - - public double getPIDDownSetpoint() { - return pidControllerDown.getSetpoint(); - } - - public double getPIDDownError() { - return pidControllerDown.getError(); - } } \ No newline at end of file