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')) { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy') files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/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 // 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", "version": "2025.0",
"command": { "command": {
"type": "sequential", "type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": { "data": {
"commands": [ "commands": [
{ {
@@ -10,48 +14,45 @@
"pathName": "Back To Shoot Red" "pathName": "Back To Shoot Red"
} }
}, },
{
"type": "named",
"data": {
"name": "climber_init"
}
}
]
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "shoot" "name": "shoot"
} }
}, },
{
"type": "wait",
"data": {
"waitTime": 0.25
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {
"pathName": "Shoot to Climb Step1 Red" "pathName": "Shoot to Climb Step1 Red"
} }
}, },
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "climb_up" "name": "climb_up"
} }
}, },
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {
"pathName": "Climb Step2 Red" "pathName": "Climb Step2 Red"
} }
}, },
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {
@@ -61,7 +62,7 @@
{ {
"type": "wait", "type": "wait",
"data": { "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": { "data": {
"name": "intaker_down" "name": "intaker_down"
} }
},
{
"type": "named",
"data": {
"name": "climber_init"
}
} }
] ]
} }
@@ -29,6 +35,12 @@
"name": "shoot" "name": "shoot"
} }
}, },
{
"type": "named",
"data": {
"name": "start_intake_and_conveyor"
}
},
{ {
"type": "parallel", "type": "parallel",
"data": { "data": {
@@ -38,12 +50,6 @@
"data": { "data": {
"pathName": "Shoot to Depot Red" "pathName": "Shoot to Depot Red"
} }
},
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
} }
] ]
} }
@@ -52,12 +58,6 @@
"type": "parallel", "type": "parallel",
"data": { "data": {
"commands": [ "commands": [
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {
@@ -74,13 +74,7 @@
{ {
"type": "named", "type": "named",
"data": { "data": {
"name": "intaker_up" "name": "AutoAim"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shoot Red"
} }
} }
] ]
@@ -89,7 +83,32 @@
{ {
"type": "named", "type": "named",
"data": { "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": { "anchor": {
"x": 15.9, "x": 15.9,
"y": 3.365 "y": 3.465
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 15.522355156858092, "x": 15.522355156858092,
"y": 3.365 "y": 3.465
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -17,11 +17,11 @@
{ {
"anchor": { "anchor": {
"x": 15.517, "x": 15.517,
"y": 3.365 "y": 3.465
}, },
"prevControl": { "prevControl": {
"x": 15.77785583456356, "x": 15.77785583456356,
"y": 3.365 "y": 3.465
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
src/main/deploy/pathplanner/paths/Climb Step3 Blue.path
+6 -6
View File
@@ -8,8 +8,8 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 1.3229961597714217, "x": 1.323,
"y": 4.702614323435373 "y": 4.704
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 4.604 "y": 4.604
}, },
"prevControl": { "prevControl": {
"x": 0.7939837109919805, "x": 0.7925211774538647,
"y": 4.592976954720596 "y": 4.604
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 0.0
}, },
"reversed": false, "reversed": false,
"folder": "Blue Middle", "folder": "Blue Middle",
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 0.0
}, },
"useDefaultConstraints": false "useDefaultConstraints": false
} }
src/main/deploy/pathplanner/paths/Climb Step3 Red.path
+4 -4
View File
@@ -4,12 +4,12 @@
{ {
"anchor": { "anchor": {
"x": 15.517, "x": 15.517,
"y": 3.365 "y": 3.465
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 15.767, "x": 15.767,
"y": 3.365 "y": 3.465
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -17,11 +17,11 @@
{ {
"anchor": { "anchor": {
"x": 15.517, "x": 15.517,
"y": 3.465 "y": 3.565
}, },
"prevControl": { "prevControl": {
"x": 15.239842716853756, "x": 15.239842716853756,
"y": 3.465 "y": 3.565
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "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, "prevControl": null,
"nextControl": { "nextControl": {
"x": 1.5191395924055082, "x": 1.39,
"y": 5.975819656893259 "y": 5.969
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 0.54, "x": 0.64,
"y": 5.969 "y": 5.969
}, },
"prevControl": { "prevControl": {
"x": 0.2904111750287428, "x": 0.39,
"y": 5.954667573496888 "y": 5.969
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 0.5, "maxVelocity": 0.25,
"maxAcceleration": 3.0, "maxAcceleration": 0.5,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
src/main/deploy/pathplanner/paths/Depot Intake Red.path
+2 -2
View File
@@ -16,11 +16,11 @@
}, },
{ {
"anchor": { "anchor": {
"x": 16.0, "x": 15.9,
"y": 2.1 "y": 2.1
}, },
"prevControl": { "prevControl": {
"x": 15.75, "x": 15.65,
"y": 2.1 "y": 2.1
}, },
"nextControl": null, "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, "prevControl": null,
"nextControl": { "nextControl": {
"x": 16.024211489072027, "x": 16.18921636103803,
"y": 1.797481861769637 "y": 1.647541846848796
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 4.0 "y": 4.0
}, },
"prevControl": { "prevControl": {
"x": 13.313026105373341, "x": 13.247149348869588,
"y": 4.165954098256533 "y": 4.238251681778358
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 1.5, "maxVelocity": 2.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 180.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, "prevControl": null,
"nextControl": { "nextControl": {
"x": 13.951314803780512, "x": 14.25,
"y": 3.7446544288446453 "y": 4.0
}, },
"isLocked": true, "isLocked": true,
"linkedName": null "linkedName": null
@@ -17,15 +17,15 @@
{ {
"anchor": { "anchor": {
"x": 14.567, "x": 14.567,
"y": 3.3650694444444444 "y": 3.4
}, },
"prevControl": { "prevControl": {
"x": 14.32971291116739, "x": 14.088847622018482,
"y": 3.4437767342805516 "y": 3.5461858523613685
}, },
"nextControl": { "nextControl": {
"x": 14.804287088832611, "x": 14.806076188990758,
"y": 3.2863621546083372 "y": 3.326907073819316
}, },
"isLocked": true, "isLocked": true,
"linkedName": null "linkedName": null
@@ -36,8 +36,8 @@
"y": 3.365 "y": 3.365
}, },
"prevControl": { "prevControl": {
"x": 15.593954129696199, "x": 15.572533463420765,
"y": 3.387182687738287 "y": 3.399470424413628
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path
+3 -3
View File
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 1.5, "maxVelocity": 2.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 0.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, "prevControl": null,
"nextControl": { "nextControl": {
"x": 13.72461981440462, "x": 13.699377937127288,
"y": 3.8902460070118927 "y": 3.7771427571164287
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 2.1 "y": 2.1
}, },
"prevControl": { "prevControl": {
"x": 15.201612527520673, "x": 15.073650526430217,
"y": 2.25153433233069 "y": 2.4474296685616617
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 1.5, "maxVelocity": 2.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 360.0, "maxAngularVelocity": 360.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 180.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" "Red Middle"
], ],
"autoFolders": [ "autoFolders": [
"Red Middle" "Red Middle",
"Blue Middle"
], ],
"defaultMaxVel": 1.5, "defaultMaxVel": 1.5,
"defaultMaxAccel": 3.0, "defaultMaxAccel": 3.0,
src/main/java/frc/robot/Constants.java
+2 -1
View File
@@ -18,6 +18,7 @@ public final class Constants {
} }
public class Shooter { 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() public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0) .withKP(0.0).withKI(0).withKD(0)
.withKV(0.099).withKA(0.0).withKS(0.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); .withKV(0.00).withKA(0.00).withKS(0.00);
//如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI) //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
public static final double IntakeGetterSpeedforAuto = 0.4; public static final double IntakeGetterSpeedforAuto = 0.5;
// 单位RPS // 单位RPS
public static final double positionToleranceRotations = 0.00; public static final double positionToleranceRotations = 0.00;
//rotater容差 //rotater容差
src/main/java/frc/robot/Robot.java
+3 -1
View File
@@ -50,7 +50,9 @@ public class Robot extends TimedRobot {
} }
@Override @Override
public void autonomousPeriodic() {} public void autonomousPeriodic() {
m_robotContainer.driveControls.pushDistanceData();
}
@Override @Override
public void autonomousExit() {} 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 ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper); private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper); private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
@@ -82,7 +82,6 @@ public class RobotContainer {
configueClimber(); configueClimber();
bingdingLED(); bingdingLED();
configueIntaker(); configueIntaker();
configureBindings();
Autocommand.preNameCommands( Autocommand.preNameCommands(
climberSubsystem, climberSubsystem,
IntakerSubsystem, IntakerSubsystem,
@@ -134,6 +133,12 @@ public class RobotContainer {
// Start：强行使用metaTag2全场定位 // Start：强行使用metaTag2全场定位
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2)); 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 //LM 按下时autoaimming，松开时alltelop
// controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
// controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
@@ -169,8 +174,7 @@ public class RobotContainer {
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100))); controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets())); controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand())); 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 绑定 */ /* LED 绑定 */
@@ -202,19 +206,6 @@ public class RobotContainer {
controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem)); 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() { public ClimberSubsystem getClimberSubsystem() {
return climberSubsystem; 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.subsystems.ClimberSubsystem;
import frc.robot.RobotStatusManager; import frc.robot.RobotStatusManager;
import frc.robot.subsystems.IntakerSubsystem; import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.LimelightSupplier;
import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.Constants; import frc.robot.Constants;
@@ -34,16 +33,19 @@ import frc.robot.Constants.RobotStatus;
NamedCommands.registerCommand("climb_up", climb_up(climber)); NamedCommands.registerCommand("climb_up", climb_up(climber));
NamedCommands.registerCommand("climb_down", climb_down(climber)); NamedCommands.registerCommand("climb_down", climb_down(climber));
NamedCommands.registerCommand("start_intake", start_intake(intaker)); 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("start_intake_timelimit", start_intake_timelimit(intaker));
NamedCommands.registerCommand("stop_intake", stop_intake(intaker)); NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
NamedCommands.registerCommand("intaker_down", intaker_down(intaker)); NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
NamedCommands.registerCommand("intaker_up", intaker_up(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("shoot", shoot(shooter));
NamedCommands.registerCommand("Shake", shake(driveControls)); NamedCommands.registerCommand("Shake", shake(driveControls));
NamedCommands.registerCommand("AutoAim", autoAim(driveControls)); NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls)); NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls));
NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber)); NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber));
NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls));
} }
@@ -58,12 +60,9 @@ import frc.robot.Constants.RobotStatus;
}, climber)); }, climber));
} }
public static Command climb_down(ClimberSubsystem climber) { public static Command climb_down(ClimberSubsystem climber) {
return new InstantCommand(() -> { return new RunCommand(() -> {
climber.markAutoClimbUsedInAuto(); // 标记自动攀爬已使用
}, climber)
.andThen(new RunCommand(() -> {
climber.climbDown(); // 控制爬升器下降 climber.climbDown(); // 控制爬升器下降
}, climber).withTimeout(2.0)) }, climber).withTimeout(2.0)
.andThen(new InstantCommand(() -> { .andThen(new InstantCommand(() -> {
climber.captureHoldPosition(); // 捕获当前爬升器位置以保持 climber.captureHoldPosition(); // 捕获当前爬升器位置以保持
climber.holdPosition(); // 停止并锁定当前爬升器位置 climber.holdPosition(); // 停止并锁定当前爬升器位置
@@ -71,51 +70,70 @@ import frc.robot.Constants.RobotStatus;
} }
public static Command start_intake(IntakerSubsystem intaker) { public static Command start_intake(IntakerSubsystem intaker) {
return new RunCommand(() -> { return new InstantCommand(() -> {
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球 intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
}, intaker); }, 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) { public static Command start_intake_timelimit(IntakerSubsystem intaker) {
return new RunCommand(() -> { return new RunCommand(() -> {
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 吸球机构下降 intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 吸球机构下降
}, intaker).withTimeout(3).andThen(new InstantCommand(() -> { }, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> {
intaker.setIntakerGetterSpeed(0); // 停止旋转 intaker.setIntakerGetterSpeed(0); // 停止旋转
}, intaker)); }, intaker));
} }
public static Command stop_intake(IntakerSubsystem intaker) { public static Command stop_intake(IntakerSubsystem intaker) {
return new InstantCommand(() -> { return new RunCommand(() -> {
intaker.setIntakerGetterSpeed(0); // 停止吸球 intaker.setIntakerGetterSpeed(0); // 停止吸球
}, intaker); }, intaker);
} }
public static Command intaker_down(IntakerSubsystem intaker) { public static Command intaker_down(IntakerSubsystem intaker) {
return new RunCommand(() -> { return new RunCommand(() -> {
intaker.IntakerDownNonStop(); // 吸球机构下降 intaker.IntakerDownNonStop(); // 吸球机构下降
}, intaker).withTimeout(1.0).andThen(new InstantCommand(() -> { }, intaker).withTimeout(0.6)
intaker.stopIntakerotater(); // 停止旋转
.andThen(new RunCommand(() -> {
intaker.IntakerUpNonStop(); // 吸球机构上升
}, intaker).withTimeout(0.1))
.andThen(new InstantCommand(() -> {
intaker.stopIntakerotater(); // 停止
}, intaker)); }, intaker));
} }
public static Command intaker_up(IntakerSubsystem intaker) { public static Command intaker_up(IntakerSubsystem intaker) {
return new InstantCommand(() -> { return new RunCommand(() -> {
intaker.IntakerUpNonStop(); // 吸球机构上升 intaker.IntakerUpNonStop(); // 吸球机构上升
}, intaker).withTimeout(0.8).andThen(new InstantCommand(() -> { }, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
intaker.stopIntakerotater();; // 停止旋转 intaker.stopIntakerotater(); // 停止旋转
}, intaker)); }, intaker));
} }
public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) { // public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
return Commands.sequence( // return Commands.sequence(
new InstantCommand(() -> { // new InstantCommand(() -> {
LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); // LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
}), // }),
Commands.runEnd( // Commands.runEnd(
() -> { // () -> {
drivetrain.driveToAprilTag(); // drivetrain.driveToAprilTag();
}, // },
() -> { // () -> {
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); // LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
}, // },
drivetrain // drivetrain
).until(() -> drivetrain.isAlignedToAprilTag())); // ).until(() -> drivetrain.isAlignedToAprilTag()));
} // }
public static Command shoot(ShooterSubsystem shooter) { public static Command shoot(ShooterSubsystem shooter) {
return new RunCommand(() -> { return new RunCommand(() -> {
@@ -126,7 +144,7 @@ import frc.robot.Constants.RobotStatus;
}else{ }else{
shooter.outputBackboard(); shooter.outputBackboard();
} }
}, shooter).withTimeout(1.0).andThen(new RunCommand(() -> { }, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
shooter.setFlywheelSpeedByRPS(64); shooter.setFlywheelSpeedByRPS(64);
shooter.setBackboardPosition(175); shooter.setBackboardPosition(175);
if(shooter.isBackboardAtTarget()){ if(shooter.isBackboardAtTarget()){
@@ -135,7 +153,7 @@ import frc.robot.Constants.RobotStatus;
shooter.outputBackboard(); shooter.outputBackboard();
} }
shooter.setConveyorSpeedByRPS(25); shooter.setConveyorSpeedByRPS(25);
}, shooter).withTimeout(3.0).andThen(new InstantCommand(() -> { }, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
shooter.setFlywheelSpeedByRPS(0); // 停止射击 shooter.setFlywheelSpeedByRPS(0); // 停止射击
//shooter.setBackboardPosition(0); // 恢复背板位置 //shooter.setBackboardPosition(0); // 恢复背板位置
shooter.setConveyorSpeedByRPS(0); // 停止输送 shooter.setConveyorSpeedByRPS(0); // 停止输送
@@ -148,6 +166,9 @@ import frc.robot.Constants.RobotStatus;
public static Command autoShoot(ShooterControls shooterControl) { public static Command autoShoot(ShooterControls shooterControl) {
return shooterControl.autoShootToHubCommand(); return shooterControl.autoShootToHubCommand();
} }
public static Command resetBackboard0Command(ShooterControls shooterControl) {
return shooterControl.resetBackboard0Command();
}
public static Command shake(DriveControls driveControls) { public static Command shake(DriveControls driveControls) {
return driveControls.shakeCommand(); 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 edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.LimelightSupplier;
public class ClimberControls { public class ClimberControls {
src/main/java/frc/robot/controls/DriveControls.java
+51 -20
View File
@@ -20,7 +20,6 @@ import frc.robot.Constants.DriveMode;
import frc.robot.Constants.RobotStatus; import frc.robot.Constants.RobotStatus;
import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.LimelightSupplier;
import java.util.Optional; import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
@@ -138,12 +137,12 @@ public class DriveControls {
vomega = 0; vomega = 0;
break; break;
case PassingBall: case PassingBall:
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]); vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break; break;
case AutoAimming: case AutoAimming:
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); //LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
vx = -driver.getLeftY() * maxSpeed * slowDriveScale; vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
@@ -153,7 +152,7 @@ public class DriveControls {
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
break; break;
case Climbing: case Climbing:
alignToClimb(); // alignToClimb();
break; break;
case CrossingBump: case CrossingBump:
double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]); 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() { public Command getAllianceColorCommand() {
return Commands.runOnce(this::setTeamColors); return Commands.runOnce(this::setTeamColors);
} }
public void alignToClimb(){ // public void alignToClimb(){
LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex); // LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
if(!drivetrain.isAlignedToAprilTag()){ // if(!drivetrain.isAlignedToAprilTag()){
drivetrain.driveToAprilTag(); // drivetrain.driveToAprilTag();
} // }
else{ // else{
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex); // LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
drivetrain.stop(); // drivetrain.stop();
} // }
} // }
public Command autoAimCommand() { public Command autoAimCommand() {
return drivetrain.applyRequest(() -> { return drivetrain.applyRequest(() -> {
double[] distanceAndRotation = calculateDistanceAndRotationToHub(); double[] distanceAndRotation = calculateDistanceAndRotationToHub();
@@ -438,22 +437,54 @@ public class DriveControls {
new InstantCommand(timer::restart), new InstantCommand(timer::restart),
drivetrain.applyRequest(() -> { drivetrain.applyRequest(() -> {
double t = timer.get(); double t = timer.get();
boolean positive = ((int) (t / switchPeriod)) % 2 == 0; int phase = ((int) (t / switchPeriod)) % 4;
double vy = positive ? amplitude : -amplitude;
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 return robotCentric
.withVelocityX(0.0) .withVelocityX(vx)
.withVelocityY(vy) .withVelocityY(vy)
.withRotationalRate(0.0); .withRotationalRate(vomega);
}, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime), }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
new InstantCommand(() -> { new InstantCommand(() -> {
timer.stop(); timer.stop();
autoControlNetworkTable.getEntry("ShakeVy").setDouble(0.0);
drivetrain.stop(); drivetrain.stop();
}, drivetrain) }, drivetrain)
); );
} }
public Command shakeCommand() { 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() { public Command defaultIntakerCommand() {
return Commands.run(() -> { return Commands.run(() -> {
double getterspeed = 0; double getterspeed = 0;
double conveyoyspeedforintake = 0;
double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent(); double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
if (controller.getLeftTriggerAxis() > 0.1) { if (controller.getLeftTriggerAxis() > 0.1) {
getterspeed = controller.getLeftTriggerAxis() * 0.3 + 0.3; getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake);
} }
IntakerSubsystem.setIntakerGetterSpeed(getterspeed); IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
@@ -87,9 +85,6 @@ public class IntakerControls {
IntakerSubsystem.IntakerUpNonStop(); IntakerSubsystem.IntakerUpNonStop();
} else if(controller.getLeftY() > 0.5){ } else if(controller.getLeftY() > 0.5){
IntakerSubsystem.IntakerDownNonStop(); IntakerSubsystem.IntakerDownNonStop();
// double rotaterspeed = -controller.getLeftY() * 0.3 + 0.2;
// IntakerSubsystem.setIntakerRotater(rotaterspeed);
} else { } else {
IntakerSubsystem.stopIntakerotater(); IntakerSubsystem.stopIntakerotater();
} }
src/main/java/frc/robot/controls/ShooterControls.java
+29 -11
View File
@@ -47,14 +47,16 @@ public class ShooterControls {
}else{ }else{
shooterSubsystem.outputBackboard(); shooterSubsystem.outputBackboard();
} }
shooterSubsystem.setFlywheelSpeedByRPS(0);
if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
else{shooterSubsystem.setConveyorSpeedByRPS(0);}
break; break;
case PassingBall: case PassingBall:
var xP = shooterSubsystem.getDistanceToPassball(); var xP = shooterSubsystem.getDistanceToPassball();
xP = Math.max(1.2, Math.min(4.5, xP)); xP = Math.max(2.5, Math.min(10.0, 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; 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; 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 = +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;
backboardPosition += backboardPositionOffset;
conveyorSpeed = Constants.Shooter.conveyorSpeed; conveyorSpeed = Constants.Shooter.conveyorSpeed;
if (controller.getRightTriggerAxis() < 0.1) { if (controller.getRightTriggerAxis() < 0.1) {
flywheelSpeed = 0; flywheelSpeed = 0;
@@ -63,6 +65,7 @@ public class ShooterControls {
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){ if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
conveyorSpeed = 0; conveyorSpeed = 0;
} }
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition); shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -90,6 +93,7 @@ public class ShooterControls {
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){ if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
conveyorSpeed = 0; conveyorSpeed = 0;
} }
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition); shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -182,7 +186,7 @@ public class ShooterControls {
shooterSubsystem.isBackboardAtTarget() shooterSubsystem.isBackboardAtTarget()
&& shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9 && shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
) )
.withTimeout(1.0) .withTimeout(0.3)
.andThen(new RunCommand(() -> { .andThen(new RunCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed); shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
shooterSubsystem.setBackboardPosition(backboardTargetPosition); shooterSubsystem.setBackboardPosition(backboardTargetPosition);
@@ -191,12 +195,8 @@ public class ShooterControls {
} else { } else {
shooterSubsystem.outputBackboard(); shooterSubsystem.outputBackboard();
} }
if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) { shooterSubsystem.setConveyorSpeedByRPS(25);
shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed); }, shooterSubsystem).withTimeout(5.0))
} else {
shooterSubsystem.setConveyorSpeedByRPS(0.0);
}
}, shooterSubsystem).withTimeout(3.0))
.andThen(new InstantCommand(() -> { .andThen(new InstantCommand(() -> {
shooterSubsystem.setFlywheelSpeedByRPS(0.0); shooterSubsystem.setFlywheelSpeedByRPS(0.0);
shooterSubsystem.setConveyorSpeedByRPS(0.0); shooterSubsystem.setConveyorSpeedByRPS(0.0);
@@ -204,4 +204,22 @@ public class ShooterControls {
}, shooterSubsystem)); }, shooterSubsystem));
}, Set.of(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(); stopMotor();
} }
public void markAutoClimbUsedInAuto() {
autoClimbUsedInAuto = true;
}
public boolean wasAutoClimbUsedInAuto() {
return autoClimbUsedInAuto;
}
public void clearAutoClimbUsedInAutoFlag() {
autoClimbUsedInAuto = false;
}
public void stopAll() { public void stopAll() {
climberMotor.set(0); climberMotor.set(0);
} }
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+124 -113
View File
@@ -173,6 +173,17 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs); 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() { private void fuseLimelightmt2() {
Pose2d current = getState().Pose; Pose2d current = getState().Pose;
@@ -276,10 +287,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略） // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
//Pose2d current = getState().Pose; //Pose2d current = getState().Pose;
if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) { // if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
//System.out.println("NoUsingMetaTag2ForTooChangingPose"); // //System.out.println("NoUsingMetaTag2ForTooChangingPose");
return false; // return false;
} // }
return true; return true;
} }
@@ -287,128 +298,128 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
return Math.max(min, Math.min(max, value)); return Math.max(min, Math.min(max, value));
} }
public String driveToAprilTag() { // public String driveToAprilTag() {
System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)); // System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
System.out.println("TV = " + LimelightSupplier.isTargetVisible()); // System.out.println("TV = " + LimelightSupplier.isTargetVisible());
if (!LimelightSupplier.isTargetVisible()) { // if (!LimelightSupplier.isTargetVisible()) {
isAlignedToAprilTag = false; // isAlignedToAprilTag = false;
// //stop();
// return "No Target";
// }
// 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;
// }
// }
// if (!foundDesiredTag) {
// isAlignedToAprilTag = false;
// stop(); // stop();
return "No Target"; // return "No Desired Tag";
}
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;
}
}
if (!foundDesiredTag) {
isAlignedToAprilTag = false;
stop();
return "No Desired Tag";
}
// ===== 读取原始误差 =====
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;
if (isAlignedToAprilTag) {
stop();
return "Aligned";
}
// ===== 计算输出 =====
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;
// 最小输出，避免快到目标时不动
if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
}
// 最大输出限幅
vx = clamp(vx,
-Constants.Limelight.AutoClimbMaxVX,
Constants.Limelight.AutoClimbMaxVX);
}
// 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(ry) > Constants.Limelight.AutoClimbToleranceRY) { // // ===== 读取原始误差 =====
// omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY; // 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;
// if (isAlignedToAprilTag) {
// stop();
// return "Aligned";
// } // }
// omega = omegaFromTx + omegaFromRy; // // ===== 计算输出 =====
// double vx = 0.0;
// double vy = 0.0;
// double omega = 0.0;
// // 大偏角时直接给较明显的转速，先把朝向拉回来 // // 1) 前后控制：根据 tz 前进/后退
// if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) { // if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
// omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega); // vx = tz * Constants.Limelight.AutoClimbKpTZ;
// }
// // 最小输出，避免转不动 // // 最小输出，避免快到目标时不动
// if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) { // if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
// omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega); // vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
// } // }
// // 最大输出限幅 // // 最大输出限幅
// omega = clamp(omega, // vx = clamp(vx,
// -Constants.Limelight.AutoClimbMaxOmega, // -Constants.Limelight.AutoClimbMaxVX,
// Constants.Limelight.AutoClimbMaxOmega); // Constants.Limelight.AutoClimbMaxVX);
// }
Rotation2d ClimbcurrentAngle = getState().Pose.getRotation(); // // 2) 旋转控制：tx 为主，ry 为辅
double climbCurrentRad = ClimbcurrentAngle.getRadians(); // // tx 负责“镜头中心对准 tag”
double TargetAngleforClimbing = Math.toRadians(0); // // ry 负责“机器人姿态微调”
double Climberror = TargetAngleforClimbing - climbCurrentRad; // // double omegaFromTx = 0.0;
if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) { // // double omegaFromRy = 0.0;
omega = 0.0;
} else { // // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
omega = Climberror * Constants.AutoPositioning.TurningkP; // // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
} // // }
// // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
// // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
// // }
// // omega = omegaFromTx + omegaFromRy;
// // // 大偏角时直接给较明显的转速，先把朝向拉回来
// // 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);
// // }
// // // 最大输出限幅
// // 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;
// }
// ===== 调试输出 ===== // // ===== 调试输出 =====
driveNetworkTable.getEntry("ClimbTX").setDouble(tx); // driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
driveNetworkTable.getEntry("ClimbTZ").setDouble(tz); // driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
driveNetworkTable.getEntry("ClimbRY").setDouble(ry); // driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx); // driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega); // driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag); // 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(){ // public boolean isAlignedToAprilTag(){
return isAlignedToAprilTag; // return isAlignedToAprilTag;
} // }
@Override @Override
public void periodic() { public void periodic() {
@@ -420,10 +431,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees()); driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name()); driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC); driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag()); //positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX()); // positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY()); // positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ()); // positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false); positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0 // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
LimelightHelpers.SetRobotOrientation( LimelightHelpers.SetRobotOrientation(
src/main/java/frc/robot/subsystems/IntakerSubsystem.java
+3 -16
View File
@@ -83,26 +83,13 @@ public class IntakerSubsystem extends SubsystemBase{
stopIntakerotater(); stopIntakerotater();
} }
public void IntakerDownNonStop(){ public void IntakerDownNonStop(){
double currentPosition = getIntakerotaterPosition(); intakerotater.set(-0.1);
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();
}
} }
public void IntakerUpNonStop(){ public void IntakerUpNonStop(){
double currentPosition = getIntakerotaterPosition(); intakerotater.set(0.2);
if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
// move up while below the upper limit
intakerotater.set(0.3);
} else {
// at or above upper limit: stop
stopIntakerotater();
}
} }
public void setIntakerRotater(double rotaterspeed){ public void setIntakerRotater(double rotaterspeed){
intakerotater.set(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 com.ctre.phoenix6.signals.MotorAlignmentValue;
import edu.wpi.first.math.controller.ProfiledPIDController; 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.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableInstance;