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

After champs code

Browse Source
This commit is contained in:
Team 2648 2025-04-22 21:35:52 -04:00
parent cca7d68766
commit 4386de4d4d
9 changed files with 159 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto
View File

@ -45,6 +45,25 @@
"data": {
"name": "Shoot Algae"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "HP Pickup"
}
},
{
"type": "path",
"data": {
"pathName": "Post-Barge Backup"
}
}
]
}
}
]
}

8
src/main/deploy/pathplanner/paths/30 Right to HP.path
View File

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.2587090163934425,
"y": 7.08186475409836
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": {
"x": 2.4191352739726026,
"y": 6.654494863013699
"x": 2.287270519874242,
"y": 6.774371912194027
},
"nextControl": null,
"isLocked": false,

8
src/main/deploy/pathplanner/paths/HP to 330 Right.path
View File

@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 1.2587090163934425,
"y": 7.08186475409836
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": null,
"nextControl": {
"x": 2.550917920503032,
"y": 6.856479480125757
"x": 2.0019467213114757,
"y": 6.434528688524591
},
"isLocked": false,
"linkedName": "HP Left Position"

8
src/main/deploy/pathplanner/paths/HP to K.path
View File

@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 1.2587090163934425,
"y": 7.08186475409836
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": null,
"nextControl": {
"x": 2.058518835616438,
"y": 6.624443493150685
"x": 1.9266540815180775,
"y": 6.744320542331013
},
"isLocked": false,
"linkedName": "HP Left Position"

8
src/main/deploy/pathplanner/paths/L Backup.path
View File

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.2587090163934425,
"y": 7.08186475409836
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": {
"x": 1.4625000000000001,
"y": 6.8061475409836065
"x": 1.3306352459016395,
"y": 6.926024590163935
},
"nextControl": null,
"isLocked": false,

54
src/main/deploy/pathplanner/paths/Post-Barge Backup.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.032020547945206,
"y": 4.761258561643835
},
"prevControl": null,
"nextControl": {
"x": 6.71311475409836,
"y": 4.9480532786885245
},
"isLocked": false,
"linkedName": "Pre-Barge"
},
{
"anchor": {
"x": 5.933913934426228,
"y": 5.247745901639345
},
"prevControl": {
"x": 6.641188524590164,
"y": 5.043954918032786
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": false
}

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

@ -339,7 +339,7 @@ public class RobotContainer {
),
manipulatorPivot.maintainPosition()
.withTimeout(
0.01
0.1
)
)
);
@ -354,7 +354,7 @@ public class RobotContainer {
NamedCommands.registerCommand(
"Shoot Algae",
shootAlgae()
shootAlgae().withTimeout(2)
);
NamedCommands.registerCommand(
@ -367,6 +367,11 @@ public class RobotContainer {
"Pickup Algae L2",
moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.raceWith(manipulator.runManipulator(() -> 0.85, false))
.andThen(
elevator.maintainPosition()
.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
//Dont you need a holdPosition call?
);
}
@ -625,9 +630,9 @@ public class RobotContainer {
private Command shootAlgae(){
return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition()));
}

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

@ -56,6 +56,8 @@ public class ElevatorConstants {
public static final double kVoltageLimit = 7;
public static final double kVoltageLimitAlgae = 9;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = .25;
public static final double kSysIDStepVolts = 3;

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

@ -253,6 +253,64 @@ public class Elevator extends SubsystemBase {
}
}
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
if (setpoint.getAsDouble() == 0) {
return startRun(() -> {
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
else{
elevatorMotor1.setVoltage(
MathUtil.clamp(
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
.andThen(runManualElevator(() -> -.5)
.until(() -> encoder.getPosition() == 0));
} else {
return startRun(() -> {
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
else{
elevatorMotor1.setVoltage(
MathUtil.clamp(
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
}
}
/**
* Returns the current encoder position
*