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

elevator pid work, but crash

Browse Source
This commit is contained in:
Team 2648 2025-02-22 13:22:00 -05:00
parent 44a036f420
commit 87e7eb4974
4 changed files with 24 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -71,6 +71,7 @@ public class RobotContainer {
configureButtonBindings();
//elevatorSysIDBindings();
//elevatorOnlyBindings();
configureNamedCommands();
@ -86,6 +87,13 @@ public class RobotContainer {
operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
}*/
private void elevatorOnlyBindings(){
elevator.setDefaultCommand(elevator.maintainPosition());
manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());
driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
}
private void configureButtonBindings() {
//Default commands
climberPivot.setDefaultCommand(
@ -139,7 +147,7 @@ public class RobotContainer {
startingConfig()
);
driver.y().onTrue(drivetrain.zeroHeading());
driver.y().whileTrue(drivetrain.zeroHeading());
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
@ -177,19 +185,15 @@ public class RobotContainer {
);
operator.a().onTrue(
coralIntakeRoutine()
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
);
operator.x().onTrue(
algaeIntakeRoutine(true)
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
);
operator.b().onTrue(
algaeIntakeRoutine(false)
);
operator.y().whileTrue(
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
);
}
@ -398,8 +402,8 @@ public class RobotContainer {
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
elevator.maintainPosition());*/
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
.andThen(elevator.goToSetpoint(() -> elevatorPosition))
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
.raceWith(elevator.maintainPosition()));
}
@SuppressWarnings("unused")

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 = 52.0;
public static final double kL4Position = 49.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 = 52.0; //actual is 52
public static final double kMaxHeight = 51.0; //actual is 51
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = .25;

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

@ -34,7 +34,7 @@ public class ManipulatorPivotConstants {
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
public static final double kEncoderOffset = 0.7815;
public static final double kEncoderOffset = 0.781;
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
public static final double kL1Position = Units.degreesToRadians(0.0);
@ -46,7 +46,7 @@ public class ManipulatorPivotConstants {
public static final double kProcesserPosition = Units.degreesToRadians(175.0);
public static final double kNetPosition = Units.degreesToRadians(175.0);
/**The closest position to the elevator brace without hitting it */
public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = Units.degreesToRadians(175.0);

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

@ -141,7 +141,7 @@ public class Elevator extends SubsystemBase {
public Command maintainPosition() {
return startRun(() -> {
System.out.println("maintain position");
},
() -> {
@ -184,10 +184,13 @@ public class Elevator extends SubsystemBase {
return startRun(() -> {
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
System.out.println("go to setpoint");
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());