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

pid gain scheduling

Browse Source
This commit is contained in:
Tylr-J42 2025-02-22 02:48:58 -05:00
parent 3dafb3c269
commit a145c290fd
4 changed files with 68 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -138,6 +138,8 @@ public class RobotContainer {
driver.start().and(driver.back()).onTrue( driver.start().and(driver.back()).onTrue(
startingConfig() startingConfig()
); );
driver.y().onTrue(drivetrain.zeroHeading());
driver.povDown().whileTrue(climberPivot.runPivot(-0.5)); driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
driver.povUp().whileTrue(climberPivot.runPivot(0.5)); driver.povUp().whileTrue(climberPivot.runPivot(0.5));
@ -189,7 +191,6 @@ public class RobotContainer {
operator.y().whileTrue( operator.y().whileTrue(
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position) elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
); );
} }
@ -245,12 +246,12 @@ public class RobotContainer {
sensorTab.addDouble("ElevMotor2", elevator::getMotor2) sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
.withWidget(BuiltInWidgets.kGraph); .withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint) sensorTab.addDouble("Elevator setpoint", elevator::getPIDUpSetpoint)
.withSize(1, 1) .withSize(1, 1)
.withPosition(5, 0) .withPosition(5, 0)
.withWidget(BuiltInWidgets.kTextView); .withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator error", elevator::getPIDError) sensorTab.addDouble("Elevator error", elevator::getPIDUpError)
.withSize(1, 1) .withSize(1, 1)
.withPosition(5, 1) .withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView); .withWidget(BuiltInWidgets.kTextView);
@ -382,7 +383,6 @@ public class RobotContainer {
* @param armPosition The target rotation of the arm * @param armPosition The target rotation of the arm
* @return Moves the elevator and arm to the setpoints * @return Moves the elevator and arm to the setpoints
*/ */
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) { private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); .andThen(manipulatorPivot.goToSetpoint(() -> armPosition));

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

@ -43,14 +43,14 @@ public class ElevatorConstants {
public static final double kL1Position = 0; public static final double kL1Position = 0;
public static final double kL2Position = 14.5; public static final double kL2Position = 14.5;
public static final double kL3Position = 29.0; public static final double kL3Position = 29.0;
public static final double kL4Position = 53.0; public static final double kL4Position = 52.0;
public static final double kL4TransitionPosition = 40.0; public static final double kL4TransitionPosition = 40.0;
public static final double kL2AlgaePosition = 22.0; public static final double kL2AlgaePosition = 22.0;
public static final double kL3AlgaePosition = 39.0; public static final double kL3AlgaePosition = 39.0;
public static final double kProcessorPosition = 4.0; public static final double kProcessorPosition = 4.0;
/**The position of the top of the elevator brace */ /**The position of the top of the elevator brace */
public static final double kBracePosition = 0; public static final double kBracePosition = 0;
public static final double kMaxHeight = 47.5; //actual is 53 public static final double kMaxHeight = 52.0; //actual is 52
// 1, 7, 10 are the defaults for these, change as necessary // 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = .25; public static final double kSysIDRampRate = .25;

9
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -248,9 +248,12 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.resetEncoders(); m_rearRight.resetEncoders();
} }
/** Zeroes the heading of the robot. */ /** Zeroes the heading of the robot.
public void zeroHeading() { * @return */
gyro.reset(); public Command zeroHeading() {
return run(() -> {
gyro.reset();
});
} }
public double getGyroValue() { public double getGyroValue() {

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

@ -24,7 +24,8 @@ public class Elevator extends SubsystemBase {
private DigitalInput bottomLimitSwitch; private DigitalInput bottomLimitSwitch;
private PIDController pidController; private PIDController pidControllerUp;
private PIDController pidControllerDown;
private ElevatorFeedforward feedForward; private ElevatorFeedforward feedForward;
@ -57,14 +58,23 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kBottomLimitSwitchID ElevatorConstants.kBottomLimitSwitchID
); );
pidController = new PIDController( pidControllerDown = new PIDController(
ElevatorConstants.kDownControllerP, ElevatorConstants.kDownControllerP,
ElevatorConstants.kDownControllerI, ElevatorConstants.kDownControllerI,
ElevatorConstants.kDownControllerD ElevatorConstants.kDownControllerD
); );
pidController.setSetpoint(0); pidControllerDown.setSetpoint(0);
pidController.setTolerance(ElevatorConstants.kAllowedError); pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
pidControllerUp = new PIDController(
ElevatorConstants.kUpControllerP,
ElevatorConstants.kUpControllerI,
ElevatorConstants.kUpControllerD
);
pidControllerUp.setSetpoint(0);
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
feedForward = new ElevatorFeedforward( feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS, ElevatorConstants.kFeedForwardS,
@ -132,53 +142,30 @@ public class Elevator extends SubsystemBase {
return startRun(() -> { return startRun(() -> {
//pidController.reset();
// pidController.setSetpoint(encoder.getPosition());
}, },
() -> { () -> {
/*if (!pidController.atSetpoint()) { if (!pidControllerUp.atSetpoint()) {
elevatorMotor1.setVoltage( if(encoder.getPosition()>pidControllerUp.getSetpoint()){
pidController.calculate( elevatorMotor1.setVoltage(
encoder.getPosition() pidControllerUp.calculate(
) + feedForward.calculate(0) encoder.getPosition()
); ) + feedForward.calculate(0)
} else { );
elevatorMotor1.setVoltage( }else{
feedForward.calculate(0) elevatorMotor1.setVoltage(
); pidControllerDown.calculate(
}*/ encoder.getPosition()
) + feedForward.calculate(0)
elevatorMotor1.setVoltage( );
feedForward.calculate(0) }
);
});
/*
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0)
);
*/
/*
if (!pidController.atSetpoint()) {
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0)
);
} else { } else {
elevatorMotor1.setVoltage( elevatorMotor1.setVoltage(
feedForward.calculate(0) feedForward.calculate(0)
); );
} }
});
});*/
} }
/** /**
@ -192,26 +179,28 @@ public class Elevator extends SubsystemBase {
return startRun(() -> { return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble()); pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidController.reset(); pidControllerDown.setSetpoint(setpoint.getAsDouble());
pidControllerUp.reset();
pidControllerDown.reset();
}, },
() -> { () -> {
if (!pidController.atSetpoint()) { if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( elevatorMotor1.setVoltage(
pidController.calculate( pidControllerUp.calculate(
encoder.getPosition(), encoder.getPosition()
setpoint.getAsDouble()
) + feedForward.calculate(0) ) + feedForward.calculate(0)
); );
} else { else{
elevatorMotor1.setVoltage( elevatorMotor1.setVoltage(
feedForward.calculate(0) pidControllerDown.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
); );
} }
}).until(() -> pidController.atSetpoint());
}).until(() -> pidControllerUp.atSetpoint());
/* /*
elevatorMotor1.setVoltage( elevatorMotor1.setVoltage(
@ -293,11 +282,19 @@ public class Elevator extends SubsystemBase {
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage(); return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
} }
public double getPIDSetpoint() { public double getPIDUpSetpoint() {
return pidController.getSetpoint(); return pidControllerUp.getSetpoint();
} }
public double getPIDError() { public double getPIDUpError() {
return pidController.getError(); return pidControllerUp.getError();
}
public double getPIDDownSetpoint() {
return pidControllerDown.getSetpoint();
}
public double getPIDDownError() {
return pidControllerDown.getError();
} }
} }