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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -4,8 +4,8 @@
"holonomicMode": true, "holonomicMode": true,
"pathFolders": [], "pathFolders": [],
"autoFolders": [], "autoFolders": [],
"defaultMaxVel": 4.0, "defaultMaxVel": 2.0,
"defaultMaxAccel": 2.5, "defaultMaxAccel": 1.0,
"defaultMaxAngVel": 540.0, "defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 400.0, "defaultMaxAngAccel": 400.0,
"defaultNominalVoltage": 12.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.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; 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.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.Trigger;
@ -126,9 +127,7 @@ public class RobotContainer {
manipulator.setDefaultCommand( manipulator.setDefaultCommand(
manipulator.runUntilCollected( manipulator.runManipulator(() -> 0.0, false)
() -> 0.0
)
); );
//Driver inputs //Driver inputs
@ -160,19 +159,21 @@ public class RobotContainer {
driver.start().whileTrue(drivetrain.resetToVision()); driver.start().whileTrue(drivetrain.resetToVision());
driver.rightBumper().whileTrue( driver.rightBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose( drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180)) () -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
) ))
); );
driver.leftBumper().whileTrue( driver.leftBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose( drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180)) () -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
) ))
); );
//Operator inputs //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.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.a().onTrue( operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition) safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
); );
operator.x().onTrue( operator.x().onTrue(
@ -233,13 +234,14 @@ public class RobotContainer {
} }
private void configureNamedCommands() { private void configureNamedCommands() {
new EventTrigger("Lift L4").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)); //new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); 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("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).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5)); 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)); 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)); 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; import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants { public class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 4; public static final double kMaxSpeedMetersPerSecond = 5.5;
public static final double kMaxAccelerationMetersPerSecondSquared = 2; public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 6; public static final double kPXController = 3;
public static final double kPYController = 6; public static final double kPYController = 3;
public static final double kPThetaController = 5.5; public static final double kPThetaController = 5.5;
// Constraint for the motion profiled robot angle controller // 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 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 kCoralIntakePosition = 0;
public static final double kL1Position = 0; public static final double kL1Position = 14;
public static final double kL2Position = 11; public static final double kL2Position = 11;
public static final double kL3Position = 27; public static final double kL3Position = 27;
public static final double kL4Position = 50.5; 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 kStartingPosition = Units.degreesToRadians(90);
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+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 kL2Position = Units.degreesToRadians(22.0+90);
public static final double kL3Position = 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); 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} {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 = new AHRS(NavXComType.kMXP_SPI);
gyro.reset();
m_estimator = new SwerveDrivePoseEstimator( m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics, DrivetrainConstants.kDriveKinematics,
@ -110,7 +109,7 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition() m_rearRight.getPosition()
}, },
new Pose2d(), 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)) VecBuilder.fill(1, 1, Units.degreesToRadians(360))
); );
@ -173,9 +172,9 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition() 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.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){ if(vision.getOrangeDist() < 60){
@ -197,7 +196,7 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected()){ if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){ 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()){ 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. * @param pose The pose to which to set the odometry.
*/ */
public void resetOdometry(Pose2d pose) { public void resetOdometry(Pose2d pose) {
m_estimator.resetPosition( m_estimator.resetPosition(
Rotation2d.fromDegrees(getGyroValue()), Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
@ -451,7 +451,7 @@ public class Drivetrain extends SubsystemBase {
} }
public Command resetToVision(){ public Command resetToVision(){
return run(() -> { return runOnce(() -> {
m_estimator.resetPose(orangePose2d); m_estimator.resetPose(orangePose2d);
}); });
} }