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

working on two piece auto

Browse Source
This commit is contained in:
Team 2648 2025-03-22 18:07:15 -04:00
parent 8cc14b4cc3
commit 073b2ab754
22 changed files with 171 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
src/main/deploy/pathplanner/autos/Two Coral Left Event.auto Normal file
View File

@ -0,0 +1,74 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to 30 Right"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "30 Right to HP"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to 330 Left"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "K Approach"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

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

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.174795081967213,
"y": 7.321618852459016
"x": 1.1987704918032787,
"y": 7.189754098360656
},
"prevControl": {
"x": 2.031259123063103,
"y": 6.375000701774084
"x": 2.055234532899169,
"y": 6.243135947675724
},
"nextControl": null,
"isLocked": false,
@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

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

@ -45,8 +45,8 @@
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/E to HP.path
View File

@ -52,8 +52,8 @@
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/H Backup.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

12
src/main/deploy/pathplanner/paths/HP to 330 Left.path
View File

@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 1.174795081967213,
"y": 7.321618852459016
"x": 1.1987704918032787,
"y": 7.189754098360656
},
"prevControl": null,
"nextControl": {
"x": 2.4670039860768025,
"y": 7.096233578486413
"x": 2.490979395912868,
"y": 6.964368824388053
},
"isLocked": false,
"linkedName": "HP Left Position"
@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/HP to D.path
View File

@ -52,8 +52,8 @@
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

8
src/main/deploy/pathplanner/paths/J Approach.path
View File

@ -17,11 +17,11 @@
{
"anchor": {
"x": 4.974897540983607,
"y": 5.115881147540984
"y": 5.235758196721312
},
"prevControl": {
"x": 5.058811475409836,
"y": 5.403586065573771
"y": 5.523463114754099
},
"nextControl": null,
"isLocked": false,
@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

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

@ -4,12 +4,12 @@
{
"anchor": {
"x": 4.974897540983607,
"y": 5.115881147540984
"y": 5.235758196721312
},
"prevControl": null,
"nextControl": {
"x": 5.11855393929902,
"y": 5.383513333352196
"y": 5.503390382532524
},
"isLocked": false,
"linkedName": "J"
@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

16
src/main/deploy/pathplanner/paths/K Approach.path
View File

@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 3.7970635805425386,
"y": 5.68234215716238
"x": 3.6663590190101236,
"y": 5.6134008057257505
},
"isLocked": false,
"linkedName": "Before K"
},
{
"anchor": {
"x": 3.8720286885245896,
"y": 5.043954918032787
"x": 3.7290765411991873,
"y": 5.091647695859483
},
"prevControl": {
"x": 3.836065573770491,
"y": 5.295696721311475
"x": 3.693113426445089,
"y": 5.343389499138171
},
"nextControl": null,
"isLocked": false,
@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/New Path.path
View File

@ -44,8 +44,8 @@
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

6
src/main/deploy/pathplanner/paths/Right Start to E.path
View File

@ -9,7 +9,7 @@
"prevControl": null,
"nextControl": {
"x": 6.461044520547946,
"y": 0.478938356164384
"y": 0.4789383561643841
},
"isLocked": false,
"linkedName": null
@ -52,8 +52,8 @@
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

36
src/main/deploy/pathplanner/paths/Start to 30 Right.path
View File

@ -3,38 +3,50 @@
"waypoints": [
{
"anchor": {
"x": 7.588217213114754,
"y": 7.537397540983607
"x": 7.1686475409836055,
"y": 7.573360655737705
},
"prevControl": null,
"nextControl": {
"x": 6.3534836065573765,
"y": 6.51844262295082
"x": 5.933913934426228,
"y": 6.5544057377049185
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.286577868852459,
"y": 5.955020491803278
"x": 4.974897540983607,
"y": 5.235758196721312
},
"prevControl": {
"x": 5.872579581181226,
"y": 6.826510217830675
"x": 5.560899253312374,
"y": 6.107247922748709
},
"nextControl": null,
"isLocked": false,
"linkedName": "Before J"
"linkedName": "J"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/Start to H.path
View File

@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/paths/fein.path
View File

@ -49,8 +49,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxVelocity": 2.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

4
src/main/deploy/pathplanner/settings.json
View File

@ -4,8 +4,8 @@
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 4.0,
"defaultMaxAccel": 2.5,
"defaultMaxVel": 2.0,
"defaultMaxAccel": 1.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 400.0,
"defaultNominalVoltage": 12.0,

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

@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@ -126,9 +127,7 @@ public class RobotContainer {
manipulator.setDefaultCommand(
manipulator.runUntilCollected(
() -> 0.0
)
manipulator.runManipulator(() -> 0.0, false)
);
//Driver inputs
@ -160,19 +159,21 @@ public class RobotContainer {
driver.start().whileTrue(drivetrain.resetToVision());
driver.rightBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
)
))
);
driver.leftBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
)
))
);
//Operator inputs
@ -209,7 +210,7 @@ public class RobotContainer {
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
);
operator.x().onTrue(
@ -233,13 +234,14 @@ public class RobotContainer {
}
private void configureNamedCommands() {
new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runUntilCollected(() -> 0.0).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).until(() -> manipulator.getCoralBeamBreak() == false).andThen(manipulator.runManipulator(() -> 0, false)).withTimeout(0.1));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
.andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1)));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
}

8
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -11,13 +11,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 4;
public static final double kMaxAccelerationMetersPerSecondSquared = 2;
public static final double kMaxSpeedMetersPerSecond = 5.5;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 6;
public static final double kPYController = 6;
public static final double kPXController = 3;
public static final double kPYController = 3;
public static final double kPThetaController = 5.5;
// Constraint for the motion profiled robot angle controller

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

@ -42,7 +42,7 @@ public class ElevatorConstants {
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;
public static final double kL1Position = 14;
public static final double kL2Position = 11;
public static final double kL3Position = 27;
public static final double kL4Position = 50.5;

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

@ -38,7 +38,7 @@ public class ManipulatorPivotConstants {
public static final double kStartingPosition = Units.degreesToRadians(90);
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
public static final double kL1Position = Units.degreesToRadians(0.0+90);
public static final double kL1Position = Units.degreesToRadians(246);
public static final double kL2Position = Units.degreesToRadians(22.0+90);
public static final double kL3Position = Units.degreesToRadians(22.0+90);
public static final double kL4Position = Units.degreesToRadians(45.0+90);

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

@ -59,6 +59,6 @@ public class VisionConstants {
{4.993, 2.816, 5.272, 2.996}
};
public static final double latencyFudge = 0;
public static final double latencyFudge = 0.0;
}

12
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -98,7 +98,6 @@ public class Drivetrain extends SubsystemBase {
);
gyro = new AHRS(NavXComType.kMXP_SPI);
gyro.reset();
m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics,
@ -110,7 +109,7 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
},
new Pose2d(),
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
);
@ -173,9 +172,9 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
});
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
@ -197,7 +196,7 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360)));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
@ -255,6 +254,7 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_estimator.resetPosition(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
@ -451,7 +451,7 @@ public class Drivetrain extends SubsystemBase {
}
public Command resetToVision(){
return run(() -> {
return runOnce(() -> {
m_estimator.resetPose(orangePose2d);
});
}