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

2 piece worked in match

Browse Source
This commit is contained in:
Team 2648 2025-04-04 11:09:09 -04:00
parent 83db16794f
commit e98b3a585e
5 changed files with 40 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 1.0549180327868852, "x": 1.2587090163934425,
"y": 7.285655737704918 "y": 7.08186475409836
}, },
"prevControl": { "prevControl": {
"x": 1.9113820738827756, "x": 2.115173057489333,
"y": 6.339037587019986 "y": 6.135246603413428
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -34,7 +34,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 5.0, "maxVelocity": 5.0,
"maxAcceleration": 2.5, "maxAcceleration": 2.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

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

@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.0549180327868852, "x": 1.2587090163934425,
"y": 7.285655737704918 "y": 7.08186475409836
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.3471269368964744, "x": 2.550917920503032,
"y": 7.060270463732315 "y": 6.856479480125757
}, },
"isLocked": false, "isLocked": false,
"linkedName": "HP Left Position" "linkedName": "HP Left Position"
}, },
{ {
"anchor": { "anchor": {
"x": 3.5723360655737704, "x": 3.6442622950819668,
"y": 4.996004098360656 "y": 5.031967213114754
}, },
"prevControl": { "prevControl": {
"x": 3.256796682012127, "x": 3.328722911520323,
"y": 5.521903070963395 "y": 5.557866185717494
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -46,7 +46,7 @@
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.5, "maxVelocity": 3.5,
"maxAcceleration": 1.25, "maxAcceleration": 1.5,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

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

@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.0549180327868852, "x": 1.2587090163934425,
"y": 7.285655737704918 "y": 7.08186475409836
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 1.5703893442622952, "x": 1.7741803278688524,
"y": 6.734221311475409 "y": 6.530430327868851
}, },
"isLocked": false, "isLocked": false,
"linkedName": "HP Left Position" "linkedName": "HP Left Position"

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

@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.5723360655737704, "x": 3.6442622950819668,
"y": 4.996004098360656 "y": 5.031967213114754
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.4398363305729753, "x": 3.5117625600811717,
"y": 5.208003674361928 "y": 5.243966789116026
}, },
"isLocked": false, "isLocked": false,
"linkedName": "L" "linkedName": "L"
}, },
{ {
"anchor": { "anchor": {
"x": 1.0549180327868852, "x": 1.2587090163934425,
"y": 7.285655737704918 "y": 7.08186475409836
}, },
"prevControl": { "prevControl": {
"x": 1.258709016393443, "x": 1.4625000000000001,
"y": 7.009938524590164 "y": 6.8061475409836065
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,

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

@ -180,14 +180,16 @@ public class Drivetrain extends SubsystemBase {
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60 && getVelocity() < 2){ if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && getVelocity() < 2){ }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 60 && getVelocity() > 2){ }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && getVelocity() > 2){ }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
}else if(getVelocity() < 1.5){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.06, 0.06, Units.degreesToRadians(360)));
} }
// if the detected tags match your alliances reef tags use their pose estimates // if the detected tags match your alliances reef tags use their pose estimates
@ -206,14 +208,16 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60 && getVelocity() < 2){ if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && getVelocity() < 2){ }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 60 && getVelocity() > 2){ }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && getVelocity() > 2){ }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
}else if(getVelocity() < 1.5){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.06, 0.06, 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()){