admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Have Climber

Browse Source
This commit is contained in:
OnDYCLaptop
2026-03-15 08:42:33 +08:00
parent be6342b597
commit 6196da8a23
44 changed files with 1644 additions and 439 deletions
Download Patch File Download Diff File Expand all files Collapse all files
build.gradle
+1 -1
View File
@@ -33,7 +33,7 @@ deploy {
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/deploy'
deleteOldFiles = false // Change to true to delete files on roboRIO that no
deleteOldFiles = true // Change to true to delete files on roboRIO that no
// longer exist in deploy directory of this project
}
}
src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto
+25
View File
@@ -0,0 +1,25 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Blue"
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/8 Balls Middle Start.auto → src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto
View File
src/main/deploy/pathplanner/autos/Depot Auto Blue.auto
+88
View File
@@ -0,0 +1,88 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intaker_down"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "To Depot Blue"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{
"type": "path",
"data": {
"pathName": "Depot Intake Blue"
}
}
]
}
},
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intaker_up"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shoot Blue"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Depot Auto.auto → src/main/deploy/pathplanner/autos/Depot Auto Red.auto
View File
src/main/deploy/pathplanner/autos/New Auto.auto
+31
View File
@@ -0,0 +1,31 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "New Path"
}
},
{
"type": "named",
"data": {
"name": "AutoAim"
}
},
{
"type": "named",
"data": {
"name": "AutoShoot"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto
+67
View File
@@ -0,0 +1,67 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Blue"
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
},
{
"type": "wait",
"data": {
"waitTime": 0.25
}
},
{
"type": "path",
"data": {
"pathName": "Shoot to Climb Step1 Blue"
}
},
{
"type": "named",
"data": {
"name": "climb_up"
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step2 Blue"
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step3 Blue"
}
},
{
"type": "wait",
"data": {
"waitTime": 0.25
}
},
{
"type": "named",
"data": {
"name": "climb_down"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Climb.auto → src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto
+20 -19
View File
@@ -2,6 +2,10 @@
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
@@ -10,48 +14,45 @@
"pathName": "Back To Shoot Red"
}
},
{
"type": "named",
"data": {
"name": "climber_init"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
},
{
"type": "wait",
"data": {
"waitTime": 0.25
}
},
{
"type": "path",
"data": {
"pathName": "Shoot to Climb Step1 Red"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "named",
"data": {
"name": "climb_up"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step2 Red"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "path",
"data": {
@@ -61,7 +62,7 @@
{
"type": "wait",
"data": {
"waitTime": 2.0
"waitTime": 0.25
}
},
{
src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto
+158
View File
@@ -0,0 +1,158 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Blue"
}
},
{
"type": "named",
"data": {
"name": "intaker_down"
}
},
{
"type": "named",
"data": {
"name": "climber_init"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "shoot"
}
},
{
"type": "named",
"data": {
"name": "intaker_down"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "start_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Shoot to Depot Blue"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Depot Intake Blue"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoAim"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "stop_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoShoot"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "blue depot go 1"
}
},
{
"type": "named",
"data": {
"name": "start_intake"
}
},
{
"type": "named",
"data": {
"name": "reset_backboard0"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto
+139
View File
@@ -0,0 +1,139 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Red"
}
},
{
"type": "named",
"data": {
"name": "intaker_down"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Shoot to Depot Red"
}
},
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{
"type": "path",
"data": {
"pathName": "Depot Intake Red"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intaker_up"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shoot Red"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "AutoAim"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoShoot"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoShoot"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Depot.auto → src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto
+39 -20
View File
@@ -19,6 +19,12 @@
"data": {
"name": "intaker_down"
}
},
{
"type": "named",
"data": {
"name": "climber_init"
}
}
]
}
@@ -29,6 +35,12 @@
"name": "shoot"
}
},
{
"type": "named",
"data": {
"name": "start_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
@@ -38,12 +50,6 @@
"data": {
"pathName": "Shoot to Depot Red"
}
},
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
}
]
}
@@ -52,12 +58,6 @@
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{
"type": "path",
"data": {
@@ -74,13 +74,7 @@
{
"type": "named",
"data": {
"name": "intaker_up"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shoot Red"
"name": "AutoAim"
}
}
]
@@ -89,7 +83,32 @@
{
"type": "named",
"data": {
"name": "shoot"
"name": "stop_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoShoot"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
}
]
src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto
+151
View File
@@ -0,0 +1,151 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Blue"
}
},
{
"type": "named",
"data": {
"name": "intaker_down"
}
},
{
"type": "named",
"data": {
"name": "climber_init"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
},
{
"type": "named",
"data": {
"name": "start_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Shoot to Depot Blue"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Depot Intake Blue"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoAim"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "stop_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoShoot"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Depot To Climb Blue"
}
},
{
"type": "named",
"data": {
"name": "climb_up"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step2 Blue"
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step3 Blue"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto
+157
View File
@@ -0,0 +1,157 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Red"
}
},
{
"type": "named",
"data": {
"name": "intaker_down"
}
},
{
"type": "named",
"data": {
"name": "climber_init"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot"
}
},
{
"type": "named",
"data": {
"name": "start_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Shoot to Depot Red"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Depot Intake Red"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoAim"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "stop_intake_and_conveyor"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AutoShoot"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Depot To Climb Red"
}
},
{
"type": "named",
"data": {
"name": "climb_up"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step2 Red"
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step3 Red"
}
},
{
"type": "named",
"data": {
"name": "climb_down"
}
}
]
}
},
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Test of individual command Blue.auto
+31
View File
@@ -0,0 +1,31 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "start_intake"
}
},
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "named",
"data": {
"name": "stop_intake"
}
}
]
}
},
"resetOdom": false,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Test of individual command.auto
+25
View File
@@ -0,0 +1,25 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "start_intake_and_conveyor"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
}
]
}
},
"resetOdom": false,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/Climb Step2 Red.path
+4 -4
View File
@@ -4,12 +4,12 @@
{
"anchor": {
"x": 15.9,
"y": 3.365
"y": 3.465
},
"prevControl": null,
"nextControl": {
"x": 15.522355156858092,
"y": 3.365
"y": 3.465
},
"isLocked": false,
"linkedName": null
@@ -17,11 +17,11 @@
{
"anchor": {
"x": 15.517,
"y": 3.365
"y": 3.465
},
"prevControl": {
"x": 15.77785583456356,
"y": 3.365
"y": 3.465
},
"nextControl": null,
"isLocked": false,
src/main/deploy/pathplanner/paths/Climb Step3 Blue.path
+6 -6
View File
@@ -8,8 +8,8 @@
},
"prevControl": null,
"nextControl": {
"x": 1.3229961597714217,
"y": 4.702614323435373
"x": 1.323,
"y": 4.704
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 4.604
},
"prevControl": {
"x": 0.7939837109919805,
"y": 4.592976954720596
"x": 0.7925211774538647,
"y": 4.604
},
"nextControl": null,
"isLocked": false,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
"rotation": 0.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
"rotation": 0.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Climb Step3 Red.path
+4 -4
View File
@@ -4,12 +4,12 @@
{
"anchor": {
"x": 15.517,
"y": 3.365
"y": 3.465
},
"prevControl": null,
"nextControl": {
"x": 15.767,
"y": 3.365
"y": 3.465
},
"isLocked": false,
"linkedName": null
@@ -17,11 +17,11 @@
{
"anchor": {
"x": 15.517,
"y": 3.465
"y": 3.565
},
"prevControl": {
"x": 15.239842716853756,
"y": 3.465
"y": 3.565
},
"nextControl": null,
"isLocked": false,
src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path
+63
View File
@@ -0,0 +1,63 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 0.64,
"y": 5.969
},
"prevControl": null,
"nextControl": {
"x": 0.9200056277857709,
"y": 5.902393157337589
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.485927152317881,
"y": 5.276448675496688
},
"prevControl": {
"x": 5.220827814571248,
"y": 5.6757450331143255
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.4,
"rotationDegrees": 135.0
},
{
"waypointRelativePos": 0.68,
"rotationDegrees": 135.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Depot Intake Blue.path
+7 -7
View File
@@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 1.5191395924055082,
"y": 5.975819656893259
"x": 1.39,
"y": 5.969
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.54,
"x": 0.64,
"y": 5.969
},
"prevControl": {
"x": 0.2904111750287428,
"y": 5.954667573496888
"x": 0.39,
"y": 5.969
},
"nextControl": null,
"isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 0.5,
"maxAcceleration": 3.0,
"maxVelocity": 0.25,
"maxAcceleration": 0.5,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
src/main/deploy/pathplanner/paths/Depot Intake Red.path
+2 -2
View File
@@ -16,11 +16,11 @@
},
{
"anchor": {
"x": 16.0,
"x": 15.9,
"y": 2.1
},
"prevControl": {
"x": 15.75,
"x": 15.65,
"y": 2.1
},
"nextControl": null,
src/main/deploy/pathplanner/paths/Depot To Climb Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 0.64,
"y": 5.97
},
"prevControl": null,
"nextControl": {
"x": 0.6399999999999998,
"y": 4.9700000000000015
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.64,
"y": 4.705
},
"prevControl": {
"x": 0.6400000000000002,
"y": 5.705
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Depot To Climb Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 15.9,
"y": 2.1
},
"prevControl": null,
"nextControl": {
"x": 15.9,
"y": 3.0999999999999983
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 15.9,
"y": 3.465
},
"prevControl": {
"x": 15.9,
"y": 2.465000000000005
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.0,
"maxAcceleration": 2.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": "Red Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Depot to Shoot Red.path
+7 -7
View File
@@ -8,8 +8,8 @@
},
"prevControl": null,
"nextControl": {
"x": 16.024211489072027,
"y": 1.797481861769637
"x": 16.18921636103803,
"y": 1.647541846848796
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 4.0
},
"prevControl": {
"x": 13.313026105373341,
"y": 4.165954098256533
"x": 13.247149348869588,
"y": 4.238251681778358
},
"nextControl": null,
"isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxVelocity": 2.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/New Path.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5,
"y": 2.5
},
"prevControl": null,
"nextControl": {
"x": 3.8524186062054477,
"y": 2.382931901404091
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.0,
"y": 3.0
},
"prevControl": {
"x": 1.7628130762195064,
"y": 3.0790086272982027
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path
+9 -9
View File
@@ -8,8 +8,8 @@
},
"prevControl": null,
"nextControl": {
"x": 13.951314803780512,
"y": 3.7446544288446453
"x": 14.25,
"y": 4.0
},
"isLocked": true,
"linkedName": null
@@ -17,15 +17,15 @@
{
"anchor": {
"x": 14.567,
"y": 3.3650694444444444
"y": 3.4
},
"prevControl": {
"x": 14.32971291116739,
"y": 3.4437767342805516
"x": 14.088847622018482,
"y": 3.5461858523613685
},
"nextControl": {
"x": 14.804287088832611,
"y": 3.2863621546083372
"x": 14.806076188990758,
"y": 3.326907073819316
},
"isLocked": true,
"linkedName": null
@@ -36,8 +36,8 @@
"y": 3.365
},
"prevControl": {
"x": 15.593954129696199,
"y": 3.387182687738287
"x": 15.572533463420765,
"y": 3.399470424413628
},
"nextControl": null,
"isLocked": false,
src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path
+3 -3
View File
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxVelocity": 2.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Shoot to Depot Red.path
+7 -7
View File
@@ -8,8 +8,8 @@
},
"prevControl": null,
"nextControl": {
"x": 13.72461981440462,
"y": 3.8902460070118927
"x": 13.699377937127288,
"y": 3.7771427571164287
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 2.1
},
"prevControl": {
"x": 15.201612527520673,
"y": 2.25153433233069
"x": 15.073650526430217,
"y": 2.4474296685616617
},
"nextControl": null,
"isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxVelocity": 2.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/To Depot Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.54,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 2.539999999999999,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.14,
"y": 5.97
},
"prevControl": {
"x": 2.1399999999999992,
"y": 5.97
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/blue depot go 1.path
+95
View File
@@ -0,0 +1,95 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 0.64,
"y": 5.969
},
"prevControl": null,
"nextControl": {
"x": 1.678433584781481,
"y": 5.9229373186851895
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.6943708609271524,
"y": 7.381829470198674
},
"prevControl": {
"x": 2.178364289572292,
"y": 7.226083943133918
},
"nextControl": {
"x": 3.0531960414062747,
"y": 7.490133162361142
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.0,
"y": 7.2
},
"prevControl": {
"x": 5.804119228777829,
"y": 7.355340669064503
},
"nextControl": {
"x": 6.195880771222171,
"y": 7.044659330935497
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.195529801324503,
"y": 5.969
},
"prevControl": {
"x": 7.354483593458411,
"y": 5.7760396621938845
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.9,
"rotationDegrees": 180.0
},
{
"waypointRelativePos": 1.7,
"rotationDegrees": 180.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 135.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/settings.json
+2 -1
View File
@@ -7,7 +7,8 @@
"Red Middle"
],
"autoFolders": [
"Red Middle"
"Red Middle",
"Blue Middle"
],
"defaultMaxVel": 1.5,
"defaultMaxAccel": 3.0,
src/main/java/frc/robot/Constants.java
+3 -2
View File
@@ -17,7 +17,8 @@ public final class Constants {
CrossingTrench
}
public class Shooter {
public static final double conveyorSpeed =25;
public static final double conveyorSpeed = 25;
public static final double conveyorSpeedForAuto = 10;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0)
.withKV(0.099).withKA(0.0).withKS(0.0);
@@ -135,7 +136,7 @@ public final class Constants {
.withKV(0.00).withKA(0.00).withKS(0.00);
//如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
public static final double IntakeGetterSpeedforAuto = 0.4;
public static final double IntakeGetterSpeedforAuto = 0.5;
// 单位RPS
public static final double positionToleranceRotations = 0.00;
//rotater容差
src/main/java/frc/robot/Robot.java
+3 -1
View File
@@ -50,7 +50,9 @@ public class Robot extends TimedRobot {
}
@Override
public void autonomousPeriodic() {}
public void autonomousPeriodic() {
m_robotContainer.driveControls.pushDistanceData();
}
@Override
public void autonomousExit() {}
src/main/java/frc/robot/RobotContainer.java
+8 -17
View File
@@ -58,7 +58,7 @@ public class RobotContainer {
/* 控制封装/日志 */
/* ====================== */
private final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
@@ -82,7 +82,6 @@ public class RobotContainer {
configueClimber();
bingdingLED();
configueIntaker();
configureBindings();
Autocommand.preNameCommands(
climberSubsystem,
IntakerSubsystem,
@@ -134,6 +133,12 @@ public class RobotContainer {
// Start：强行使用metaTag2全场定位
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
System.out.println("Try using shooter for metatag2");
}));
controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
System.out.println("Try using intaker for metatag2");
}));
//LM 按下时autoaimming，松开时alltelop
// controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
// controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
@@ -169,8 +174,7 @@ public class RobotContainer {
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem));
controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem));
}
/* ====================== */
/* LED 绑定 */
@@ -202,19 +206,6 @@ public class RobotContainer {
controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
}
private void configureBindings() {
teleopStartTrigger.onTrue(handleClimberAtTeleopStartCommand());
}
private Command handleClimberAtTeleopStartCommand() {
return new InstantCommand(() -> {
if (climberSubsystem.wasAutoClimbUsedInAuto()) {
climberSubsystem.climbUp();
climberSubsystem.clearAutoClimbUsedInAutoFlag();
}
}, climberSubsystem);
}
public ClimberSubsystem getClimberSubsystem() {
return climberSubsystem;
}
src/main/java/frc/robot/controls/Autocommand.java
+53 -32
View File
@@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.RobotStatusManager;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.LimelightSupplier;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.Constants;
@@ -34,16 +33,19 @@ import frc.robot.Constants.RobotStatus;
NamedCommands.registerCommand("climb_up", climb_up(climber));
NamedCommands.registerCommand("climb_down", climb_down(climber));
NamedCommands.registerCommand("start_intake", start_intake(intaker));
NamedCommands.registerCommand("start_intake_and_conveyor", start_intake_and_conveyor(intaker,shooter));
NamedCommands.registerCommand("stop_intake_and_conveyor", stop_intake_and_conveyor(intaker,shooter));
NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker));
NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
//NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
NamedCommands.registerCommand("shoot", shoot(shooter));
NamedCommands.registerCommand("Shake", shake(driveControls));
NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls));
NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber));
NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls));
}
@@ -58,12 +60,9 @@ import frc.robot.Constants.RobotStatus;
}, climber));
}
public static Command climb_down(ClimberSubsystem climber) {
return new InstantCommand(() -> {
climber.markAutoClimbUsedInAuto(); // 标记自动攀爬已使用
}, climber)
.andThen(new RunCommand(() -> {
return new RunCommand(() -> {
climber.climbDown(); // 控制爬升器下降
}, climber).withTimeout(2.0))
}, climber).withTimeout(2.0)
.andThen(new InstantCommand(() -> {
climber.captureHoldPosition(); // 捕获当前爬升器位置以保持
climber.holdPosition(); // 停止并锁定当前爬升器位置
@@ -71,51 +70,70 @@ import frc.robot.Constants.RobotStatus;
}
public static Command start_intake(IntakerSubsystem intaker) {
return new RunCommand(() -> {
return new InstantCommand(() -> {
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
}, intaker);
}
public static Command start_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
return new InstantCommand(() -> {
shooter.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeedForAuto);
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
}, intaker, shooter);
}
public static Command stop_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
return new InstantCommand(() -> {
intaker.setIntakerGetterSpeed(0); // 启动吸球
shooter.setConveyorSpeedByRPS(0);
}, intaker, shooter);
}
public static Command start_intake_timelimit(IntakerSubsystem intaker) {
return new RunCommand(() -> {
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 吸球机构下降
}, intaker).withTimeout(3).andThen(new InstantCommand(() -> {
}, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> {
intaker.setIntakerGetterSpeed(0); // 停止旋转
}, intaker));
}
public static Command stop_intake(IntakerSubsystem intaker) {
return new InstantCommand(() -> {
return new RunCommand(() -> {
intaker.setIntakerGetterSpeed(0); // 停止吸球
}, intaker);
}
public static Command intaker_down(IntakerSubsystem intaker) {
return new RunCommand(() -> {
intaker.IntakerDownNonStop(); // 吸球机构下降
}, intaker).withTimeout(1.0).andThen(new InstantCommand(() -> {
intaker.stopIntakerotater(); // 停止旋转
}, intaker).withTimeout(0.6)
.andThen(new RunCommand(() -> {
intaker.IntakerUpNonStop(); // 吸球机构上升
}, intaker).withTimeout(0.1))
.andThen(new InstantCommand(() -> {
intaker.stopIntakerotater(); // 停止
}, intaker));
}
public static Command intaker_up(IntakerSubsystem intaker) {
return new InstantCommand(() -> {
return new RunCommand(() -> {
intaker.IntakerUpNonStop(); // 吸球机构上升
}, intaker).withTimeout(0.8).andThen(new InstantCommand(() -> {
intaker.stopIntakerotater();; // 停止旋转
}, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
intaker.stopIntakerotater(); // 停止旋转
}, intaker));
}
public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
return Commands.sequence(
new InstantCommand(() -> {
LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
}),
Commands.runEnd(
() -> {
drivetrain.driveToAprilTag();
},
() -> {
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
},
drivetrain
).until(() -> drivetrain.isAlignedToAprilTag()));
}
// public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
// return Commands.sequence(
// new InstantCommand(() -> {
// LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
// }),
// Commands.runEnd(
// () -> {
// drivetrain.driveToAprilTag();
// },
// () -> {
// LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
// },
// drivetrain
// ).until(() -> drivetrain.isAlignedToAprilTag()));
// }
public static Command shoot(ShooterSubsystem shooter) {
return new RunCommand(() -> {
@@ -126,7 +144,7 @@ import frc.robot.Constants.RobotStatus;
}else{
shooter.outputBackboard();
}
}, shooter).withTimeout(1.0).andThen(new RunCommand(() -> {
}, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
shooter.setFlywheelSpeedByRPS(64);
shooter.setBackboardPosition(175);
if(shooter.isBackboardAtTarget()){
@@ -135,7 +153,7 @@ import frc.robot.Constants.RobotStatus;
shooter.outputBackboard();
}
shooter.setConveyorSpeedByRPS(25);
}, shooter).withTimeout(3.0).andThen(new InstantCommand(() -> {
}, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
shooter.setFlywheelSpeedByRPS(0); // 停止射击
//shooter.setBackboardPosition(0); // 恢复背板位置
shooter.setConveyorSpeedByRPS(0); // 停止输送
@@ -148,6 +166,9 @@ import frc.robot.Constants.RobotStatus;
public static Command autoShoot(ShooterControls shooterControl) {
return shooterControl.autoShootToHubCommand();
}
public static Command resetBackboard0Command(ShooterControls shooterControl) {
return shooterControl.resetBackboard0Command();
}
public static Command shake(DriveControls driveControls) {
return driveControls.shakeCommand();
src/main/java/frc/robot/controls/ClimberControls.java
+1 -1
View File
@@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.LimelightSupplier;
public class ClimberControls {
src/main/java/frc/robot/controls/DriveControls.java
+52 -21
View File
@@ -20,7 +20,6 @@ import frc.robot.Constants.DriveMode;
import frc.robot.Constants.RobotStatus;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.LimelightSupplier;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
@@ -138,12 +137,12 @@ public class DriveControls {
vomega = 0;
break;
case PassingBall:
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AutoAimming:
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
@@ -153,7 +152,7 @@ public class DriveControls {
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
break;
case Climbing:
alignToClimb();
// alignToClimb();
break;
case CrossingBump:
double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
@@ -406,16 +405,16 @@ public class DriveControls {
public Command getAllianceColorCommand() {
return Commands.runOnce(this::setTeamColors);
}
public void alignToClimb(){
LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
if(!drivetrain.isAlignedToAprilTag()){
drivetrain.driveToAprilTag();
}
else{
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
drivetrain.stop();
}
}
// public void alignToClimb(){
// LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
// if(!drivetrain.isAlignedToAprilTag()){
// drivetrain.driveToAprilTag();
// }
// else{
// LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
// drivetrain.stop();
// }
// }
public Command autoAimCommand() {
return drivetrain.applyRequest(() -> {
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
@@ -438,22 +437,54 @@ public class DriveControls {
new InstantCommand(timer::restart),
drivetrain.applyRequest(() -> {
double t = timer.get();
boolean positive = ((int) (t / switchPeriod)) % 2 == 0;
double vy = positive ? amplitude : -amplitude;
int phase = ((int) (t / switchPeriod)) % 4;
double vx = 0.0;
double vy = 0.0;
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
switch (phase) {
case 0:
vx = amplitude; // 前
vy = 0.0;
break;
case 1:
vx = 0.0;
vy = amplitude; // 右
break;
case 2:
vx = -amplitude; // 后
vy = 0.0;
break;
case 3:
vx = 0.0;
vy = -amplitude; // 左
break;
}
return robotCentric
.withVelocityX(0.0)
.withVelocityX(vx)
.withVelocityY(vy)
.withRotationalRate(0.0);
.withRotationalRate(vomega);
}, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
new InstantCommand(() -> {
timer.stop();
autoControlNetworkTable.getEntry("ShakeVy").setDouble(0.0);
drivetrain.stop();
}, drivetrain)
);
}
}
public Command shakeCommand() {
return shakeCommand(0.25, 0.2, 3.0);
return shakeCommand(0.3, 0.15, 5.0);
}
public void pushDistanceData(){
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
// 将距离值写入 NetworkTable
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
}
}
src/main/java/frc/robot/controls/IntakerControls.java
+1 -6
View File
@@ -73,12 +73,10 @@ public class IntakerControls {
public Command defaultIntakerCommand() {
return Commands.run(() -> {
double getterspeed = 0;
double conveyoyspeedforintake = 0;
double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
if (controller.getLeftTriggerAxis() > 0.1) {
getterspeed = controller.getLeftTriggerAxis() * 0.3 + 0.3;
ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake);
getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
}
IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
@@ -87,9 +85,6 @@ public class IntakerControls {
IntakerSubsystem.IntakerUpNonStop();
} else if(controller.getLeftY() > 0.5){
IntakerSubsystem.IntakerDownNonStop();
// double rotaterspeed = -controller.getLeftY() * 0.3 + 0.2;
// IntakerSubsystem.setIntakerRotater(rotaterspeed);
} else {
IntakerSubsystem.stopIntakerotater();
}
src/main/java/frc/robot/controls/ShooterControls.java
+29 -11
View File
@@ -47,14 +47,16 @@ public class ShooterControls {
}else{
shooterSubsystem.outputBackboard();
}
shooterSubsystem.setFlywheelSpeedByRPS(0);
if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
else{shooterSubsystem.setConveyorSpeedByRPS(0);}
break;
case PassingBall:
var xP = shooterSubsystem.getDistanceToPassball();
xP = Math.max(1.2, Math.min(4.5, xP));
flywheelSpeed = -1.4964026859575532*xP*xP*xP*xP*xP*xP*xP+29.82445640241506*xP*xP*xP*xP*xP*xP-247.71603153159896*xP*xP*xP*xP*xP+1108.8727302184625*xP*xP*xP*xP-2881.517500617481*xP*xP*xP+4332.05363145275*xP*xP-3467.2508337871454*xP+1192.1508062663759;
xP = Math.max(2.5, Math.min(10.0, xP));
flywheelSpeed = +0.004960317456971805*xP*xP*xP*xP*xP*xP*xP-0.22361111097424535*xP*xP*xP*xP*xP*xP+4.234722219890569*xP*xP*xP*xP*xP-43.63194442298942*xP*xP*xP*xP+264.05555544032126*xP*xP*xP-938.6444440829168*xP*xP+1818.2047612908214*xP-1415.9999995643104;
flywheelSpeed += flywheelSpeedOffset;
backboardPosition = +34.5286998307899*xP*xP*xP*xP*xP*xP*xP*xP-788.5788623683884*xP*xP*xP*xP*xP*xP*xP+7658.073303693148*xP*xP*xP*xP*xP*xP-41191.69338972519*xP*xP*xP*xP*xP+133870.54588688645*xP*xP*xP*xP-268559.6987062158*xP*xP*xP+324254.6530324262*xP*xP-215015.48893131423*xP+59977.46545382827;
backboardPosition += backboardPositionOffset;
backboardPosition = +0.10912698402591796*xP*xP*xP*xP*xP*xP*xP-5.277777773586415*xP*xP*xP*xP*xP*xP+105.55555548303796*xP*xP*xP*xP*xP-1125.6944437656143*xP*xP*xP*xP+6865.763885174154*xP*xP*xP-23769.027765888906*xP*xP+43328.57140795847*xP-31599.99998506075;
conveyorSpeed = Constants.Shooter.conveyorSpeed;
if (controller.getRightTriggerAxis() < 0.1) {
flywheelSpeed = 0;
@@ -63,6 +65,7 @@ public class ShooterControls {
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
conveyorSpeed = 0;
}
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -90,6 +93,7 @@ public class ShooterControls {
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
conveyorSpeed = 0;
}
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -182,7 +186,7 @@ public class ShooterControls {
shooterSubsystem.isBackboardAtTarget()
&& shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
)
.withTimeout(1.0)
.withTimeout(0.3)
.andThen(new RunCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
@@ -191,12 +195,8 @@ public class ShooterControls {
} else {
shooterSubsystem.outputBackboard();
}
if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) {
shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed);
} else {
shooterSubsystem.setConveyorSpeedByRPS(0.0);
}
}, shooterSubsystem).withTimeout(3.0))
shooterSubsystem.setConveyorSpeedByRPS(25);
}, shooterSubsystem).withTimeout(5.0))
.andThen(new InstantCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(0.0);
shooterSubsystem.setConveyorSpeedByRPS(0.0);
@@ -204,4 +204,22 @@ public class ShooterControls {
}, shooterSubsystem));
}, Set.of(shooterSubsystem));
}
public Command resetBackboard0Command() {
return new RunCommand(() -> {
shooterSubsystem.setBackboardPosition(0);
if (shooterSubsystem.isBackboardAtTarget()) {
shooterSubsystem.backboardMotor.set(0);
} else {
shooterSubsystem.outputBackboard();
}
}, shooterSubsystem)
.until(() -> shooterSubsystem.isBackboardAtTarget())
.andThen(new InstantCommand(() -> {
shooterSubsystem.backboardMotor.set(0);
}));
}
}
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
-12
View File
@@ -114,18 +114,6 @@ public class ClimberSubsystem extends SubsystemBase {
stopMotor();
}
public void markAutoClimbUsedInAuto() {
autoClimbUsedInAuto = true;
}
public boolean wasAutoClimbUsedInAuto() {
return autoClimbUsedInAuto;
}
public void clearAutoClimbUsedInAutoFlag() {
autoClimbUsedInAuto = false;
}
public void stopAll() {
climberMotor.set(0);
}
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+117 -106
View File
@@ -173,6 +173,17 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
}
public void forceUsingLimelightmt2WithllName(String llString) {
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
if (mt2 == null || mt2.tagCount == 0) {
return;
}
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
}
private void fuseLimelightmt2() {
Pose2d current = getState().Pose;
@@ -276,10 +287,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
//Pose2d current = getState().Pose;
if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
//System.out.println("NoUsingMetaTag2ForTooChangingPose");
return false;
}
// if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
// //System.out.println("NoUsingMetaTag2ForTooChangingPose");
// return false;
// }
return true;
}
@@ -287,128 +298,128 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
return Math.max(min, Math.min(max, value));
}
public String driveToAprilTag() {
System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
System.out.println("TV = " + LimelightSupplier.isTargetVisible());
// public String driveToAprilTag() {
// System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
// System.out.println("TV = " + LimelightSupplier.isTargetVisible());
if (!LimelightSupplier.isTargetVisible()) {
isAlignedToAprilTag = false;
//stop();
return "No Target";
}
// if (!LimelightSupplier.isTargetVisible()) {
// isAlignedToAprilTag = false;
// //stop();
// return "No Target";
// }
RawFiducial[] fiducials =
LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
// RawFiducial[] fiducials =
// LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
boolean foundDesiredTag = false;
for (RawFiducial fiducial : fiducials) {
if (fiducial.id == 16 || fiducial.id == 32) {
foundDesiredTag = true;
break;
}
}
// boolean foundDesiredTag = false;
// for (RawFiducial fiducial : fiducials) {
// if (fiducial.id == 16 || fiducial.id == 32) {
// foundDesiredTag = true;
// break;
// }
// }
if (!foundDesiredTag) {
isAlignedToAprilTag = false;
stop();
return "No Desired Tag";
}
// if (!foundDesiredTag) {
// isAlignedToAprilTag = false;
// stop();
// return "No Desired Tag";
// }
// ===== 读取原始误差 =====
double tx = LimelightSupplier.getTX(); // deg
double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号
double ry = LimelightSupplier.getTargetRotationY(); // deg
// // ===== 读取原始误差 =====
// double tx = LimelightSupplier.getTX(); // deg
// double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号
// double ry = LimelightSupplier.getTargetRotationY(); // deg
// ===== 用原始误差判断是否已对齐 =====
isAlignedToAprilTag =
Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
// // ===== 用原始误差判断是否已对齐 =====
// isAlignedToAprilTag =
// Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
// Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
// Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
if (isAlignedToAprilTag) {
stop();
return "Aligned";
}
// if (isAlignedToAprilTag) {
// stop();
// return "Aligned";
// }
// ===== 计算输出 =====
double vx = 0.0;
double vy = 0.0;
double omega = 0.0;
// // ===== 计算输出 =====
// double vx = 0.0;
// double vy = 0.0;
// double omega = 0.0;
// 1) 前后控制：根据 tz 前进/后退
if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
vx = tz * Constants.Limelight.AutoClimbKpTZ;
// // 1) 前后控制：根据 tz 前进/后退
// if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
// vx = tz * Constants.Limelight.AutoClimbKpTZ;
// 最小输出，避免快到目标时不动
if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
}
// // 最小输出，避免快到目标时不动
// if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
// vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
// }
// 最大输出限幅
vx = clamp(vx,
-Constants.Limelight.AutoClimbMaxVX,
Constants.Limelight.AutoClimbMaxVX);
}
// // 最大输出限幅
// vx = clamp(vx,
// -Constants.Limelight.AutoClimbMaxVX,
// Constants.Limelight.AutoClimbMaxVX);
// }
// 2) 旋转控制：tx 为主，ry 为辅
// tx 负责“镜头中心对准 tag”
// ry 负责“机器人姿态微调”
// double omegaFromTx = 0.0;
// double omegaFromRy = 0.0;
// // 2) 旋转控制：tx 为主，ry 为辅
// // tx 负责“镜头中心对准 tag”
// // ry 负责“机器人姿态微调”
// // double omegaFromTx = 0.0;
// // double omegaFromRy = 0.0;
// if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
// omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
// }
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
// // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
// // }
// if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
// omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
// }
// // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
// // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
// // }
// omega = omegaFromTx + omegaFromRy;
// // omega = omegaFromTx + omegaFromRy;
// // 大偏角时直接给较明显的转速，先把朝向拉回来
// if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
// omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
// }
// // // 大偏角时直接给较明显的转速，先把朝向拉回来
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
// // omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
// // }
// // 最小输出，避免转不动
// if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
// omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
// }
// // // 最小输出，避免转不动
// // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
// // omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
// // }
// // 最大输出限幅
// omega = clamp(omega,
// -Constants.Limelight.AutoClimbMaxOmega,
// Constants.Limelight.AutoClimbMaxOmega);
// // // 最大输出限幅
// // omega = clamp(omega,
// // -Constants.Limelight.AutoClimbMaxOmega,
// // Constants.Limelight.AutoClimbMaxOmega);
Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
double climbCurrentRad = ClimbcurrentAngle.getRadians();
double TargetAngleforClimbing = Math.toRadians(0);
double Climberror = TargetAngleforClimbing - climbCurrentRad;
if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
omega = 0.0;
} else {
omega = Climberror * Constants.AutoPositioning.TurningkP;
}
// Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
// double climbCurrentRad = ClimbcurrentAngle.getRadians();
// double TargetAngleforClimbing = Math.toRadians(0);
// double Climberror = TargetAngleforClimbing - climbCurrentRad;
// if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
// omega = 0.0;
// } else {
// omega = Climberror * Constants.AutoPositioning.TurningkP;
// }
// ===== 调试输出 =====
driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
// // ===== 调试输出 =====
// driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
// driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
// driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
// driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
// driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
// driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
// ===== 下发到底盘 =====
driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
// // ===== 下发到底盘 =====
// driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
return "Driving to AprilTag";
}
// return "Driving to AprilTag";
// }
public boolean isAlignedToAprilTag(){
return isAlignedToAprilTag;
}
// public boolean isAlignedToAprilTag(){
// return isAlignedToAprilTag;
// }
@Override
public void periodic() {
@@ -420,10 +431,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
//positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
// positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
// positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
// positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
LimelightHelpers.SetRobotOrientation(
src/main/java/frc/robot/subsystems/IntakerSubsystem.java
+3 -16
View File
@@ -83,26 +83,13 @@ public class IntakerSubsystem extends SubsystemBase{
stopIntakerotater();
}
public void IntakerDownNonStop(){
double currentPosition = getIntakerotaterPosition();
if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) {
// move down (negative power) while above the lower limit
intakerotater.set(-0.05);
} else {
// at or below lower limit: stop
stopIntakerotater();
}
intakerotater.set(-0.1);
}
public void IntakerUpNonStop(){
double currentPosition = getIntakerotaterPosition();
if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
// move up while below the upper limit
intakerotater.set(0.3);
} else {
// at or above upper limit: stop
stopIntakerotater();
}
intakerotater.set(0.2);
}
public void setIntakerRotater(double rotaterspeed){
intakerotater.set(rotaterspeed);
}
src/main/java/frc/robot/subsystems/LimelightSupplier.java
-108
View File
@@ -1,108 +0,0 @@
package frc.robot.subsystems;
import frc.robot.Constants;
import java.lang.annotation.Target;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.subsystems.LimelightHelpers;
import frc.robot.subsystems.LimelightHelpers.RawFiducial;
public class LimelightSupplier extends SubsystemBase {
public LimelightSupplier() {
setPipeline(8);
}
public static double getTX() {
return LimelightHelpers.getTX(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static double getTY() {
return LimelightHelpers.getTY(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static double getTA() {
// Logic to get area from Limelight
return LimelightHelpers.getTA(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static boolean isTargetVisible() {
if(!LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker)){
return false;
}
// Logic to check if a target is visible
return LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static int getAprilTagID(){
RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
int id = -1;
for (RawFiducial fiducial : fiducials) {
id = fiducial.id; // 标签ID
}
return id;
}
public static void setPipeline(int pipeline) {
if(((int)LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)) != pipeline){
LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker, pipeline);
}
}
public static double[] getTargetPose() {
// Logic to get the target pose from Limelight
return LimelightHelpers.getBotPose_TargetSpace(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static Pose2d getBotPose2d_wpiBlue(){
return LimelightHelpers.getBotPose2d_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static double getTargetRotationY(){
if(isTargetVisible()){
return getTargetPose()[4];
}
else{
return 0.00;
}
}
public static double getTargetTZ(){
if(isTargetVisible()){
return getTargetPose()[2];
}
else{
return 0.00;
}
}
@Override
public void periodic() {
SmartDashboard.putNumber("LimelightPipeIndex", LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker));
if (isTargetVisible()){
SmartDashboard.putString("LimelightTargetVisible", "YES");
SmartDashboard.putNumber("LimelightTA", getTA());
SmartDashboard.putNumber("TargetRY", getTargetRotationY());
SmartDashboard.putNumber("TargetTX", getTX());
SmartDashboard.putNumber("TargetTZ", getTargetTZ());
RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
int id = -1;
for (RawFiducial fiducial : fiducials) {
id = fiducial.id; // 标签ID
}
SmartDashboard.putNumber("LimelightTargetID", id);
}
else {
SmartDashboard.putString("LimelightTargetVisible", "NO");
SmartDashboard.putNumber("TargetRY", 0);
SmartDashboard.putNumber("TargetTX", 0);
SmartDashboard.putNumber("TargetTZ", 0);
SmartDashboard.putNumber("LimelightTargetID", -1);
}
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+1
View File
@@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;