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

good 2 piece

Browse Source
This commit is contained in:
Team 2648
2025-04-16 17:46:16 -04:00
parent 060b39669f
commit a8a597985f
5 changed files with 21 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 3.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

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

@@ -46,7 +46,7 @@
],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 3.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

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

@@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 3.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

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

@@ -360,8 +360,13 @@ public class RobotContainer {
NamedCommands.registerCommand(
"Processor Position",
moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
.raceWith(manipulator.runManipulator(() -> 0.85, false))
);
NamedCommands.registerCommand(
"Pickup Algae L2",
moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.raceWith(manipulator.runManipulator(() -> 0.85, false))
);
}

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

@@ -111,8 +111,8 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
},
new Pose2d(),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0.5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(4000))
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
);
pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
@@ -177,17 +177,17 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
}
// if the detected tags match your alliances reef tags use their pose estimates
@@ -209,13 +209,13 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){