Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

on controller elevator PID control

Browse Source
This commit is contained in:
Tylr-J42 2025-05-19 22:49:28 -04:00
parent 42d47d6075
commit 626b92b769
3 changed files with 44 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/main/java/frc/robot/RobotContainer.java
View File

@ -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);

6
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -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;

73
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -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);
controller.setReference(
setpoint.position,
ControlType.kPosition,
ClosedLoopSlot.kSlot0,
feedForward.calculate(encoder.getVelocity())
);
}).until();
}).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);
controller.setReference(
setpoint.position,
ControlType.kPosition,
ClosedLoopSlot.kSlot0,
feedForward.calculate(setpoint.velocity)
);
}).until();
}).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();
}
}