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

merge with vision stuff

Browse Source
This commit is contained in:
Tylr-J42 2025-02-21 18:08:47 -05:00
commit 3dafb3c269
5 changed files with 208 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,6 +19,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -105,16 +106,12 @@ public class RobotContainer {
);
elevator.setDefaultCommand(
elevator.goToSetpoint(
() -> 0
)
elevator.maintainPosition()
);
manipulatorPivot.setDefaultCommand(
manipulatorPivot.runManualPivot(
() -> 0
)
manipulatorPivot.maintainPosition()
);
@ -125,7 +122,7 @@ public class RobotContainer {
);
//Driver inputs
/*
driver.start().whileTrue(
drivetrain.setXCommand()
);
@ -141,52 +138,37 @@ public class RobotContainer {
driver.start().and(driver.back()).onTrue(
startingConfig()
);
*/
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
operator.povUp().whileTrue(
elevator.goToSetpoint(() -> 50)
);
operator.povDown().whileTrue(
elevator.goToSetpoint(() -> 0)
);
/*
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 +185,11 @@ public class RobotContainer {
operator.b().onTrue(
algaeIntakeRoutine(false)
);
*/
operator.y().whileTrue(
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
);
}
@ -268,6 +254,12 @@ public class RobotContainer {
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
}
public Command getAutonomousCommand() {
@ -318,7 +310,7 @@ public class RobotContainer {
// then the elevator, then the arm again
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
@ -328,7 +320,7 @@ public class RobotContainer {
// Catch all command that's safe regardless of arm and elevator positions
} else {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
}
}
@ -342,23 +334,24 @@ public class RobotContainer {
* @return Moves the elevator and arm to the setpoints
*/
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
armPosition = MathUtil.clamp(
armPosition,
0,
ManipulatorPivotConstants.kRotationLimit
);
}
}*/
return Commands.either(
Commands.either(
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
() -> sequential
),
Commands.either(
manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
() -> sequential
),
() -> elevatorFirst
@ -369,15 +362,15 @@ public class RobotContainer {
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
if(!isL4){
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition),
manipulatorPivot.goToSetpoint(armPosition));
manipulatorPivot.goToSetpoint(() -> armPosition));
}else{
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
}
}
@ -392,7 +385,7 @@ public class RobotContainer {
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
}
@SuppressWarnings("unused")

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

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

@ -16,29 +16,29 @@ public class ManipulatorPivotConstants {
public static final int kMotorCurrentMax = 40;
public static final double kPivotConversion = 12.0/60.0 * 20.0/60.0 * 12.0/28.0;
public static final double kPivotConversion = 2 * Math.PI;
public static final double kPivotMaxVelocity = 0;
public static final double kPivotMaxVelocity = 2 * Math.PI;
public static final double kPositionalP = 0;
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);
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0.41; // calculated value 0.41
public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
public static final double kFeedForwardV = 0.68; //calculated value 0.68
public static final double kFFGravityOffset = Units.degreesToRadians(-135.0);
public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
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;
public static final double kEncoderOffset = 0.7815;
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);
@ -69,7 +69,8 @@ public class ManipulatorPivotConstants {
static {
motorConfig
.smartCurrentLimit(kMotorCurrentMax)
.idleMode(kIdleMode);
.idleMode(kIdleMode)
.inverted(true);
motorConfig.absoluteEncoder
.positionConversionFactor(kPivotConversion)
.inverted(false)

104
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,35 +129,41 @@ public class Elevator extends SubsystemBase {
* @return Sets motor voltage based on feed forward calculation.
*/
public Command maintainPosition() {
return run(() -> {
elevatorMotor1.setVoltage(feedForward.calculate(0));
});
}
/**
* Moves the elevator to a target destination (setpoint).
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(DoubleSupplier setpoint) {
double clampedSetpoint = MathUtil.clamp(
setpoint.getAsDouble(),
0,
ElevatorConstants.kMaxHeight
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)
);
}*/
pidController.reset();
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
});
return run(() -> {
/*
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0)
);
*/
/*
if (!pidController.atSetpoint()) {
elevatorMotor1.setVoltage(
@ -170,8 +177,65 @@ public class Elevator extends SubsystemBase {
feedForward.calculate(0)
);
}
});*/
}
/**
* Moves the elevator to a target destination (setpoint).
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
});
public Command goToSetpoint(DoubleSupplier setpoint) {
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
if (!pidController.atSetpoint()) {
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) + feedForward.calculate(0)
);
} else {
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}
}).until(() -> pidController.atSetpoint());
/*
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)
);
}
});*/
}
/*

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

@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ManipulatorPivotConstants;
@ -44,6 +45,9 @@ public class ManipulatorPivot extends SubsystemBase {
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD
);
pidController.setSetpoint(0);
pidController.disableContinuousInput();
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,
@ -92,22 +96,62 @@ public class ManipulatorPivot extends SubsystemBase {
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(double setpoint) {
double clampedSetpoint = MathUtil.clamp(
setpoint,
0,
ManipulatorPivotConstants.kRotationLimit
);
public Command goToSetpoint(DoubleSupplier setpoint) {
return startRun(() -> {
return run(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(
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(),
0
)
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)
);
});
}
@ -118,7 +162,7 @@ public class ManipulatorPivot extends SubsystemBase {
* @return Encoder's position in radians
*/
public double getEncoderPosition() {
return encoder.getPosition();
return Units.radiansToDegrees( encoder.getPosition());
}
/**
* Returns the encoder's velocity in radians per second
@ -126,6 +170,14 @@ public class ManipulatorPivot extends SubsystemBase {
* @return Encoder's velocity in radians per second
*/
public double getEncoderVelocity() {
return encoder.getVelocity();
return Units.radiansToDegrees(encoder.getVelocity());
}
public double getCGPosition(){
return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
}
public double getPivotOutput(){
return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
}
}