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

elevator and manipulator work invidiually, not together

Browse Source
This commit is contained in:
Team 2648 2025-02-20 18:57:39 -05:00
parent c48a53a0a5
commit f57cf77200
5 changed files with 108 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -103,18 +103,14 @@ public class RobotContainer {
() -> true
)
);
/*
elevator.setDefaultCommand(
elevator.goToSetpoint(
() -> 0
)
elevator.maintainPosition()
);
*/
manipulatorPivot.setDefaultCommand(
manipulatorPivot.runManualPivot(
() -> operator.getRightY() * 0.5
)
manipulatorPivot.maintainPosition()
);
@ -125,7 +121,7 @@ public class RobotContainer {
);
//Driver inputs
/*
driver.start().whileTrue(
drivetrain.setXCommand()
);
@ -147,46 +143,31 @@ public class RobotContainer {
driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
*/
operator.povUp().whileTrue(
manipulatorPivot.goToSetpoint(() -> 0)
);
operator.povDown().whileTrue(
manipulatorPivot.goToSetpoint(() -> (Units.degreesToRadians(110))
));
/*
operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
//Operator inputs
operator.povUp().onTrue(
moveManipulator(
safeMoveManipulator(
ElevatorConstants.kL4Position,
ManipulatorPivotConstants.kL4Position
)
);
operator.povRight().onTrue(
moveManipulator(
safeMoveManipulator(
ElevatorConstants.kL3Position,
ManipulatorPivotConstants.kL3Position
)
);
operator.povLeft().onTrue(
moveManipulator(
safeMoveManipulator(
ElevatorConstants.kL2Position,
ManipulatorPivotConstants.kL2Position
)
);
operator.povDown().onTrue(
moveManipulator(
safeMoveManipulator(
ElevatorConstants.kL1Position,
ManipulatorPivotConstants.kL1Position
)
@ -203,7 +184,11 @@ public class RobotContainer {
operator.b().onTrue(
algaeIntakeRoutine(false)
);
*/
operator.y().whileTrue(
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
);
}

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

@ -49,7 +49,7 @@ public class ElevatorConstants {
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 = 5.5;
public static final double kBracePosition = 0;
public static final double kMaxHeight = 47.5; //actual is 53
// 1, 7, 10 are the defaults for these, change as necessary

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

@ -20,7 +20,7 @@ public class ManipulatorPivotConstants {
public static final double kPivotMaxVelocity = 2 * Math.PI;
public static final double kPositionalP = 2.5;
public static final double kPositionalP = 4;
public static final double kPositionalI = 0;
public static final double kPositionalD = 0;
public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
@ -38,7 +38,7 @@ public class ManipulatorPivotConstants {
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
public static final double kL1Position = Units.degreesToRadians(0.0);
public static final double kL2Position = Units.degreesToRadians(25.0);
public static final double kL2Position = Units.degreesToRadians(60.0);
public static final double kL3Position = Units.degreesToRadians(60.0);
public static final double kL4Position = Units.degreesToRadians(45.0);
public static final double kL2AlgaePosition = Units.degreesToRadians(175.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(60.0);
public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = Units.degreesToRadians(175.0);

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

@ -62,6 +62,7 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kDownControllerI,
ElevatorConstants.kDownControllerD
);
pidController.setSetpoint(0);
pidController.setTolerance(ElevatorConstants.kAllowedError);
@ -128,9 +129,56 @@ public class Elevator extends SubsystemBase {
* @return Sets motor voltage based on feed forward calculation.
*/
public Command maintainPosition() {
return run(() -> {
elevatorMotor1.setVoltage(feedForward.calculate(0));
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)
);
} else {
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}
});*/
}
/**
@ -143,7 +191,6 @@ public class Elevator extends SubsystemBase {
public Command goToSetpoint(DoubleSupplier setpoint) {
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
@ -161,7 +208,7 @@ public class Elevator extends SubsystemBase {
feedForward.calculate(0)
);
}
});
}).until(() -> pidController.atSetpoint());

40
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -45,6 +45,7 @@ public class ManipulatorPivot extends SubsystemBase {
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD
);
pidController.setSetpoint(0);
pidController.disableContinuousInput();
@ -98,11 +99,11 @@ public class ManipulatorPivot extends SubsystemBase {
public Command goToSetpoint(DoubleSupplier setpoint) {
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
@ -115,6 +116,43 @@ public class ManipulatorPivot extends SubsystemBase {
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
*/
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}).until(() -> pidController.atSetpoint());
}
public Command maintainPosition() {
return startRun(() -> {
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
} else {
pivotMotor.setVoltage(
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
*/
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
pidController.getSetpoint()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
});
}