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(
startingConfig()
);
driver.y().onTrue(drivetrain.zeroHeading());
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
@ -189,7 +191,6 @@ public class RobotContainer {
operator.y().whileTrue(
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
);
}
@ -245,12 +246,12 @@ public class RobotContainer {
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
sensorTab.addDouble("Elevator setpoint", elevator::getPIDUpSetpoint)
.withSize(1, 1)
.withPosition(5, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator error", elevator::getPIDError)
sensorTab.addDouble("Elevator error", elevator::getPIDUpError)
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
@ -382,7 +383,6 @@ public class RobotContainer {
* @param armPosition The target rotation of the arm
* @return Moves the elevator and arm to the setpoints
*/
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.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 kL2Position = 14.5;
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 kL2AlgaePosition = 22.0;
public static final double kL3AlgaePosition = 39.0;
public static final double kProcessorPosition = 4.0;
/**The position of the top of the elevator brace */
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
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();
}
/** Zeroes the heading of the robot. */
public void zeroHeading() {
gyro.reset();
/** Zeroes the heading of the robot.
* @return */
public Command zeroHeading() {
return run(() -> {
gyro.reset();
});
}
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 PIDController pidController;
private PIDController pidControllerUp;
private PIDController pidControllerDown;
private ElevatorFeedforward feedForward;
@ -57,14 +58,23 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kBottomLimitSwitchID
);
pidController = new PIDController(
pidControllerDown = new PIDController(
ElevatorConstants.kDownControllerP,
ElevatorConstants.kDownControllerI,
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(
ElevatorConstants.kFeedForwardS,
@ -132,53 +142,30 @@ public class Elevator extends SubsystemBase {
return startRun(() -> {
//pidController.reset();
// pidController.setSetpoint(encoder.getPosition());
},
() -> {
/*if (!pidController.atSetpoint()) {
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
);
} else {
elevatorMotor1.setVoltage(
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)
);
if (!pidControllerUp.atSetpoint()) {
if(encoder.getPosition()>pidControllerUp.getSetpoint()){
elevatorMotor1.setVoltage(
pidControllerUp.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
);
}else{
elevatorMotor1.setVoltage(
pidControllerDown.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
);
}
} else {
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}
});*/
});
}
/**
@ -192,26 +179,28 @@ public class Elevator extends SubsystemBase {
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
pidControllerUp.reset();
pidControllerDown.reset();
},
() -> {
if (!pidController.atSetpoint()) {
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
pidControllerUp.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
);
} else {
else{
elevatorMotor1.setVoltage(
feedForward.calculate(0)
pidControllerDown.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
);
}
}).until(() -> pidController.atSetpoint());
}).until(() -> pidControllerUp.atSetpoint());
/*
elevatorMotor1.setVoltage(
@ -293,11 +282,19 @@ public class Elevator extends SubsystemBase {
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
}
public double getPIDSetpoint() {
return pidController.getSetpoint();
public double getPIDUpSetpoint() {
return pidControllerUp.getSetpoint();
}
public double getPIDError() {
return pidController.getError();
public double getPIDUpError() {
return pidControllerUp.getError();
}
public double getPIDDownSetpoint() {
return pidControllerDown.getSetpoint();
}
public double getPIDDownError() {
return pidControllerDown.getError();
}
}