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

Compare commits

base: Team_2648:main
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup
..
compare: Team_2648:2025-2-1-setup
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup

No commits in common. "main" and "2025-2-1-setup" have entirely different histories.
main ... 2025-2-1-s

58 changed files with 516 additions and 3865 deletions
Show Stats Expand all files Collapse all files

6
.DataLogTool/datalogtool.json
View File

@ -1,6 +0,0 @@
{
"download": {
"localDir": "C:\\Users\\infin\\Downloads",
"serverTeam": "2648"
}
}

58
.OutlineViewer/outlineviewer.json
View File

@ -1,58 +0,0 @@
{
"Clients": {
"open": true
},
"Connections": {
"open": true
},
"NetworkTables Settings": {
"mode": "Client (NT4)"
},
"client@1": {
"Publishers": {
"open": true
},
"open": true
},
"client@2": {
"open": true
},
"client@3": {
"open": true
},
"client@4": {
"Publishers": {
"open": true
},
"open": true
},
"client@5": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@2": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@3": {
"open": true
},
"shuffleboard@1": {
"open": true
},
"transitory": {
"Shuffleboard": {
"Sensors Tab": {
"open": true
},
"open": true
},
"orange_Fiducial": {
"open": true
}
}
}

1
.SysId/sysid.json
View File

@ -1 +0,0 @@
{}

8
build.gradle
View File

@ -10,11 +10,6 @@ java {
def ROBOT_MAIN_CLASS = "frc.robot.Main" def ROBOT_MAIN_CLASS = "frc.robot.Main"
task(replayWatch, type: JavaExec) {
mainClass = "org.littletonrobotics.junction.ReplayWatch"
classpath = sourceSets.main.runtimeClasspath
}
// Define my targets (RoboRIO) and artifacts (deployable files) // Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils. // This is added by GradleRIO's backing project DeployUtils.
deploy { deploy {
@ -77,9 +72,6 @@ dependencies {
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher' testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
} }
test { test {

BIN
src/main/deploy/Orchestra/doomE1M1.chrp
View File

Binary file not shown.

43
src/main/deploy/pathplanner/autos/1 L4 Center.auto
View File

@ -1,43 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "Start to H"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "path",
"data": {
"pathName": "H Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

74
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto
View File

@ -1,74 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to H"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "path",
"data": {
"pathName": "H Backup"
}
},
{
"type": "named",
"data": {
"name": "Pickup Algae L2"
}
},
{
"type": "path",
"data": {
"pathName": "HG Algae"
}
},
{
"type": "path",
"data": {
"pathName": "HG to Barge"
}
},
{
"type": "named",
"data": {
"name": "Shoot Algae"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "HP Pickup"
}
},
{
"type": "path",
"data": {
"pathName": "Post-Barge Backup"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

81
src/main/deploy/pathplanner/autos/2.5 Coral Left.auto
View File

@ -1,81 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to 30 Right"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "30 Right to HP"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to 330 Right"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "L Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

99
src/main/deploy/pathplanner/autos/3 Coral Left.auto
View File

@ -1,99 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to 30 Right"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "30 Right to HP"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to 330 Right"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "L Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Collect Coral"
}
},
{
"type": "path",
"data": {
"pathName": "HP to K"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

49
src/main/deploy/pathplanner/autos/One Coral Left.auto
View File

@ -1,49 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to 30 Right"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "J Approach"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "path",
"data": {
"pathName": "J Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

1
src/main/deploy/pathplanner/navgrid.json
View File

File diff suppressed because one or more lines are too long

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.988527397260274,
"y": 5.257106164383561
},
"prevControl": null,
"nextControl": {
"x": 4.843032786885245,
"y": 6.30266393442623
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": {
"x": 2.287270519874242,
"y": 6.774371912194027
},
"nextControl": null,
"isLocked": false,
"linkedName": "HP Left Position"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -53.97262661489646
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": -120.06858282186238
},
"useDefaultConstraints": false
}

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

@ -1,66 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.6061643835666786,
"y": 5.031720890416444
},
"prevControl": null,
"nextControl": {
"x": 3.065239726031196,
"y": 5.647773972598556
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.2510679859194649,
"y": 7.0812357195448
},
"prevControl": {
"x": 1.6678510274010594,
"y": 6.6394691780780075
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.498997995991984,
"rotationDegrees": -52.46519085612145
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "HP Pickup",
"waypointRelativePos": 0.16666666666666663,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 3.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -53.98486432191523
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": -59.99999999999999
},
"useDefaultConstraints": false
}

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

@ -1,73 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.958476027397261,
"y": 2.837970890410959
},
"prevControl": null,
"nextControl": {
"x": 5.619606164383562,
"y": 1.455607876712329
},
"isLocked": false,
"linkedName": "E"
},
{
"anchor": {
"x": 1.1569777397260272,
"y": 1.0198630136986298
},
"prevControl": {
"x": 2.7647260273972605,
"y": 1.485659246575342
},
"nextControl": null,
"isLocked": false,
"linkedName": "Right HP"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "HP Pickup",
"waypointRelativePos": 0.22857142857142831,
"endWaypointRelativePos": null,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 54.162347045721745
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 120.25643716352937
},
"useDefaultConstraints": true
}

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.758260140458621,
"y": 4.193633481772718
},
"prevControl": null,
"nextControl": {
"x": 6.112051124065178,
"y": 4.206338399805505
},
"isLocked": false,
"linkedName": "H"
},
{
"anchor": {
"x": 6.5,
"y": 4.021985060730393
},
"prevControl": {
"x": 6.250001988596495,
"y": 4.022982203795822
},
"nextControl": null,
"isLocked": false,
"linkedName": "Behind HG Face"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 0.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}

54
src/main/deploy/pathplanner/paths/HG Algae.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.5,
"y": 4.021985060730393
},
"prevControl": null,
"nextControl": {
"x": 6.224227344787433,
"y": 4.009974315068493
},
"isLocked": false,
"linkedName": "Behind HG Face"
},
{
"anchor": {
"x": 5.872671261177789,
"y": 4.021985060730393
},
"prevControl": {
"x": 6.125859753477119,
"y": 4.004837296105254
},
"nextControl": null,
"isLocked": false,
"linkedName": "HG Algae"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": true
}

71
src/main/deploy/pathplanner/paths/HG to Barge.path
View File

@ -1,71 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.872671261177789,
"y": 4.021985060730393
},
"prevControl": null,
"nextControl": {
"x": 7.829105562465259,
"y": 3.8676166530198106
},
"isLocked": false,
"linkedName": "HG Algae"
},
{
"anchor": {
"x": 7.032020547945206,
"y": 4.761258561643835
},
"prevControl": {
"x": 7.262336691900294,
"y": 4.359747737103425
},
"nextControl": null,
"isLocked": false,
"linkedName": "Pre-Barge"
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.17234468937875755,
"rotationDegrees": 180.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Processor Position",
"waypointRelativePos": 0.188095238095238,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Processor Position"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 360.0,
"maxAngularAcceleration": 300.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}

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

@ -1,66 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": null,
"nextControl": {
"x": 2.0019467213114757,
"y": 6.434528688524591
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.609900518622585,
"y": 5.005924534374863
},
"prevControl": {
"x": 3.2943611350609414,
"y": 5.531823506977602
},
"nextControl": null,
"isLocked": false,
"linkedName": "L"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.0,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.25,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -59.69923999693802
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": -53.97262661489646
},
"useDefaultConstraints": false
}

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

@ -1,73 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.187029109589041,
"y": 0.9747859589041092
},
"prevControl": null,
"nextControl": {
"x": 2.268878424657535,
"y": 1.0949914383561645
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.011857876712329,
"y": 2.837970890410959
},
"prevControl": {
"x": 3.3657534246575342,
"y": 1.9364297945205475
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.27142857142857096,
"endWaypointRelativePos": null,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lift L4"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 58.57043438516136
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 54.24611274556325
},
"useDefaultConstraints": true
}

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

@ -1,66 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": null,
"nextControl": {
"x": 1.9266540815180775,
"y": 6.744320542331013
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.9679303278688525,
"y": 5.211782786885246
},
"prevControl": {
"x": 2.975085616438356,
"y": 6.023416095890411
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.0,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -60.49491285058726
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": -53.97262661489646
},
"useDefaultConstraints": false
}

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.286577868852459,
"y": 5.955020491803278
},
"prevControl": null,
"nextControl": {
"x": 5.166700819672132,
"y": 5.679303278688525
},
"isLocked": false,
"linkedName": "Before J"
},
{
"anchor": {
"x": 4.974897540983607,
"y": 5.235758196721312
},
"prevControl": {
"x": 5.058811475409836,
"y": 5.523463114754099
},
"nextControl": null,
"isLocked": false,
"linkedName": "J"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -119.71497744813712
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.71497744813712
},
"useDefaultConstraints": true
}

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 4.974897540983607,
"y": 5.235758196721312
},
"prevControl": null,
"nextControl": {
"x": 5.11855393929902,
"y": 5.503390382532524
},
"isLocked": false,
"linkedName": "J"
},
{
"anchor": {
"x": 5.442044107776481,
"y": 6.005045141603656
},
"prevControl": {
"x": 5.268886874487802,
"y": 5.749866060967707
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -118.30075576600632
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.71497744813712
},
"useDefaultConstraints": true
}

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.35655737704918,
"y": 5.78719262295082
},
"prevControl": null,
"nextControl": {
"x": 3.402629510813402,
"y": 5.541474576217554
},
"isLocked": false,
"linkedName": "Before K"
},
{
"anchor": {
"x": 3.6322745901639335,
"y": 5.019979508196721
},
"prevControl": {
"x": 3.596311475409835,
"y": 5.271721311475409
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 1.0,
"maxAcceleration": 0.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -60.94539590092286
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -59.69923999693802
},
"useDefaultConstraints": false
}

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.609900518622585,
"y": 5.005924534374863
},
"prevControl": null,
"nextControl": {
"x": 3.47740078362179,
"y": 5.217924110376135
},
"isLocked": false,
"linkedName": "L"
},
{
"anchor": {
"x": 1.1268442622950818,
"y": 7.201741803278688
},
"prevControl": {
"x": 1.3306352459016395,
"y": 6.926024590163935
},
"nextControl": null,
"isLocked": false,
"linkedName": "HP Left Position"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 5.0,
"maxAcceleration": 3.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -53.97262661489646
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": -59.69923999693802
},
"useDefaultConstraints": false
}

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

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 7.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.756421232876712,
"y": 5.227054794520548
},
"prevControl": {
"x": 2.756421232876712,
"y": 5.227054794520548
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -59.18537788806707
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/Post-Barge Backup.path
View File

@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.032020547945206,
"y": 4.761258561643835
},
"prevControl": null,
"nextControl": {
"x": 6.71311475409836,
"y": 4.9480532786885245
},
"isLocked": false,
"linkedName": "Pre-Barge"
},
{
"anchor": {
"x": 5.933913934426228,
"y": 5.247745901639345
},
"prevControl": {
"x": 6.641188524590164,
"y": 5.043954918032786
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": false
}

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

@ -1,73 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.572945205479453,
"y": 0.4939640410958907
},
"prevControl": null,
"nextControl": {
"x": 6.461044520547946,
"y": 0.4789383561643841
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.958476027397261,
"y": 2.837970890410959
},
"prevControl": {
"x": 5.799914383561644,
"y": 1.9664811643835614
},
"nextControl": null,
"isLocked": false,
"linkedName": "E"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.4261904761904757,
"endWaypointRelativePos": null,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lift L4"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 120.25643716352937
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 90.0
},
"useDefaultConstraints": true
}

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

@ -1,66 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.150967037968244,
"y": 7.5521014571037055
},
"prevControl": null,
"nextControl": {
"x": 5.916233431410867,
"y": 6.533146539070919
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.974897540983607,
"y": 5.235758196721312
},
"prevControl": {
"x": 5.560899253312374,
"y": 6.107247922748709
},
"nextControl": null,
"isLocked": false,
"linkedName": "J"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.25,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -119.71497744813712
},
"reversed": false,
"folder": "Left Paths",
"idealStartingState": {
"velocity": 0,
"rotation": -90.0
},
"useDefaultConstraints": false
}

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

@ -1,66 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.588217213114754,
"y": 3.9890368852459024
},
"prevControl": null,
"nextControl": {
"x": 6.916905737704918,
"y": 4.036987704918033
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.758260140458621,
"y": 4.193633481772718
},
"prevControl": {
"x": 6.405596206032391,
"y": 4.157670367018619
},
"nextControl": null,
"isLocked": false,
"linkedName": "H"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.10238095238095252,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
"name": "Lift L4"
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
},
"reversed": false,
"folder": "Center",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}

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

@ -1,35 +0,0 @@
{
"robotWidth": 0.8763,
"robotLength": 0.8763,
"holonomicMode": true,
"pathFolders": [
"Left Paths",
"Center"
],
"autoFolders": [],
"defaultMaxVel": 3.5,
"defaultMaxAccel": 1.75,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 400.0,
"defaultNominalVoltage": 12.0,
"robotMass": 48.35,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.038,
"driveGearing": 4.29,
"maxDriveSpeed": 5.45,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 65.0,
"wheelCOF": 1.1,
"flModuleX": 0.31115,
"flModuleY": 0.31115,
"frModuleX": 0.31115,
"frModuleY": -0.31115,
"blModuleX": -0.31115,
"blModuleY": 0.31115,
"brModuleX": -0.31115,
"brModuleY": -0.31115,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []
}

32
src/main/java/frc/robot/Robot.java
View File

@ -4,15 +4,7 @@
package frc.robot; package frc.robot;
import org.littletonrobotics.junction.LoggedRobot; import edu.wpi.first.wpilibj.TimedRobot;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.CommandScheduler;
@ -22,31 +14,11 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
* the package after creating this project, you must also update the build.gradle file in the * the package after creating this project, you must also update the build.gradle file in the
* project. * project.
*/ */
public class Robot extends LoggedRobot { public class Robot extends TimedRobot {
private Command m_autonomousCommand; private Command m_autonomousCommand;
private RobotContainer m_robotContainer; private RobotContainer m_robotContainer;
@SuppressWarnings("resource")
public Robot() {
Logger.recordMetadata("ProjectName", "2025_Robot_Code"); // Set a metadata value
if (isReal()) {
Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
//Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
} else {
setUseTiming(false); // Run as fast as possible
String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
}
Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
}
/** /**
* This function is run when the robot is first started up and should be used for any * This function is run when the robot is first started up and should be used for any
* initialization code. * initialization code.

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

@ -4,35 +4,27 @@
package frc.robot; package frc.robot;
import frc.robot.constants.ManipulatorPivotConstants; import frc.robot.constants.ArmConstants;
import frc.robot.constants.ClimberPivotConstants;
import frc.robot.constants.ElevatorConstants; import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants; import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants; import frc.robot.subsystems.Arm;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Indexer;
import frc.robot.subsystems.Manipulator; import frc.robot.subsystems.Manipulator;
import java.util.function.IntSupplier;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
public class RobotContainer { public class RobotContainer {
private Arm arm;
private ClimberPivot climberPivot; private ClimberPivot climberPivot;
private ClimberRollers climberRollers; private ClimberRollers climberRollers;
@ -40,22 +32,17 @@ public class RobotContainer {
private Drivetrain drivetrain; private Drivetrain drivetrain;
private Elevator elevator; private Elevator elevator;
//private ElevatorSysID elevator;
//private Indexer indexer; private Indexer indexer;
private Manipulator manipulator; private Manipulator manipulator;
private ManipulatorPivot manipulatorPivot;
private CommandXboxController driver; private CommandXboxController driver;
private CommandXboxController operator; private CommandXboxController operator;
private SendableChooser<Command> autoChooser;
private IntSupplier closestTag;
public RobotContainer() { public RobotContainer() {
arm = new Arm();
climberPivot = new ClimberPivot(); climberPivot = new ClimberPivot();
climberRollers = new ClimberRollers(); climberRollers = new ClimberRollers();
@ -63,582 +50,176 @@ public class RobotContainer {
drivetrain = new Drivetrain(); drivetrain = new Drivetrain();
elevator = new Elevator(); elevator = new Elevator();
//elevator = new ElevatorSysID();
//indexer = new Indexer(); indexer = new Indexer();
manipulator = new Manipulator(); manipulator = new Manipulator();
manipulatorPivot = new ManipulatorPivot();
configureNamedCommands();
driver = new CommandXboxController(OIConstants.kDriverControllerPort); driver = new CommandXboxController(OIConstants.kDriverControllerPort);
operator = new CommandXboxController(OIConstants.kOperatorControllerPort); operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
autoChooser = AutoBuilder.buildAutoChooser();
autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));
closestTag = drivetrain::getClosestTag;
configureButtonBindings(); configureButtonBindings();
//elevatorSysIDBindings();
//elevatorOnlyBindings();
configureShuffleboard(); configureShuffleboard();
} }
/*private void elevatorSysIDBindings() {
elevator.setDefaultCommand(elevator.maintainPosition());
operator.a().whileTrue(elevator.sysIdQuasistatic(Direction.kForward));
operator.b().whileTrue(elevator.sysIdQuasistatic(Direction.kReverse));
operator.x().whileTrue(elevator.sysIdDynamic(Direction.kForward));
operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
}*/
private void configureButtonBindings() { private void configureButtonBindings() {
//Default commands arm.setDefaultCommand(
arm.goToSetpoint(0, 1)
);
climberPivot.setDefaultCommand( climberPivot.setDefaultCommand(
climberPivot.runPivot(() -> 0) climberPivot.goToAngle(0, 1)
); );
climberRollers.setDefaultCommand( climberRollers.setDefaultCommand(
climberRollers.runRoller(() -> 0) climberRollers.runRoller(0)
); );
drivetrain.setDefaultCommand( drivetrain.setDefaultCommand(
drivetrain.drive( drivetrain.drive(
() -> driver.getLeftY(), driver::getLeftY,
() -> driver.getLeftX(), driver::getLeftX,
() -> driver.getRightX(), driver::getRightX,
() -> true () -> true
) )
); );
elevator.setDefaultCommand( elevator.setDefaultCommand(
elevator.maintainPosition() elevator.runElevator(operator::getLeftY)
); );
//indexer.setDefaultCommand( indexer.setDefaultCommand(
// indexer.runIndexer(() -> 0) indexer.runIndexer(0)
//);
manipulatorPivot.setDefaultCommand(
manipulatorPivot.maintainPosition()
); );
manipulator.setDefaultCommand( manipulator.setDefaultCommand(
manipulator.runManipulator(() -> 0.0, false) manipulator.runManipulator(0)
); );
//Driver inputs //Driver inputs
driver.start().whileTrue( driver.start().whileTrue(
drivetrain.setXCommand() drivetrain.setXCommand()
); );
driver.rightTrigger().whileTrue( driver.rightTrigger().whileTrue(
manipulator.runManipulator(() -> 0.35, true) manipulator.runManipulator(1)
);
driver.leftTrigger().whileTrue(
manipulator.runUntilCollected(() -> 0.75)
//.alongWith(indexer.runIndexer(() -> .75))
.until(() -> manipulator.getCoralBeamBreak() == false)
.andThen(manipulator.retractCommand(() -> .1))
);
driver.start().and(driver.back()).onTrue(
startingConfig()
);
driver.y().whileTrue(drivetrain.zeroHeading());
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
driver.start().whileTrue(drivetrain.resetToVision());
driver.povUp().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
))
);
driver.rightBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
))
);
driver.leftBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
))
); );
//Operator inputs //Operator inputs
operator.povUp().onTrue( operator.povUp().onTrue(
safeMoveManipulator( moveManipulator(
ElevatorConstants.kL4Position, ElevatorConstants.kElevatorL4Position,
ManipulatorPivotConstants.kL4Position ArmConstants.kArmL4Position
) )
); );
operator.povRight().onTrue( operator.povRight().onTrue(
safeMoveManipulator( moveManipulator(
ElevatorConstants.kL3Position, ElevatorConstants.kElevatorL3Position,
ManipulatorPivotConstants.kL3Position ArmConstants.kArmL3Position
) )
); );
operator.povLeft().onTrue( operator.povLeft().onTrue(
safeMoveManipulator( moveManipulator(
ElevatorConstants.kL2Position, ElevatorConstants.kElevatorL2Position,
ManipulatorPivotConstants.kL2Position ArmConstants.kArmL2Position
) )
); );
operator.povDown().onTrue( operator.povDown().onTrue(
safeMoveManipulator( moveManipulator(
ElevatorConstants.kL1Position, ElevatorConstants.kElevatorL1Position,
ManipulatorPivotConstants.kL1Position ArmConstants.kArmL1Position
) )
); );
operator.back().onTrue(elevator.homeCommand());
operator.start().toggleOnTrue(
climberPivot.runPivot(() -> -operator.getRightY())
.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
operator.a().onTrue( operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition) coralIntakeRoutine()
); );
operator.x().onTrue( operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) algaeIntakeRoutine(true)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
); );
operator.b().onTrue( operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) algaeIntakeRoutine(false)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
);
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.until(() -> driver.a().getAsBoolean())
);
operator.rightTrigger().onTrue(shootAlgae());
}
private void configureNamedCommands() {
//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand(
"Drivetrain Set X",
drivetrain.setXCommand()
);
NamedCommands.registerCommand(
"Shoot Coral L4",
Commands.race(
manipulator.runManipulator(
() -> 0.4,
true
).withTimeout(
0.5
).andThen(
manipulator.runManipulator(
() -> 0.0,
false
).withTimeout(
0.01
)
),
Commands.parallel(
elevator.maintainPosition(),
manipulatorPivot.maintainPosition()
)
)
);
NamedCommands.registerCommand(
"Shoot Coral L4 Fast",
Commands.race(
manipulator.runManipulator(
() -> 1,
true
).andThen(
manipulator.runManipulator(
() -> 1,
true
).withTimeout(
0.125
)
).withTimeout(
3
).andThen(
manipulator.runManipulator(
() -> 0,
true
)
),
Commands.parallel(
elevator.maintainPosition(),
manipulatorPivot.maintainPosition()
)
)
);
NamedCommands.registerCommand(
"Collect Coral",
manipulator.runUntilCollected(
() -> 0.30
).andThen(
manipulator.runManipulator(
() -> 0,
false
).withTimeout(
0.01
)
)
);
NamedCommands.registerCommand(
"Lift L4",
safeMoveManipulator(
ElevatorConstants.kL4Position,
ManipulatorPivotConstants.kL4Position
).andThen(
elevator.maintainPosition()
.withTimeout(
0.1
),
manipulatorPivot.maintainPosition()
.withTimeout(
0.1
)
)
);
NamedCommands.registerCommand(
"HP Pickup",
safeMoveManipulator(
ElevatorConstants.kCoralIntakePosition,
ManipulatorPivotConstants.kStartingPosition
)
);
NamedCommands.registerCommand(
"Shoot Algae",
shootAlgae().withTimeout(2)
);
NamedCommands.registerCommand(
"Processor Position",
moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.raceWith(manipulator.runManipulator(() -> 0.85, false))
);
NamedCommands.registerCommand(
"Pickup Algae L2",
moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.raceWith(manipulator.runManipulator(() -> 0.85, false))
.andThen(
elevator.maintainPosition()
.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
//Dont you need a holdPosition call?
); );
} }
//creates tabs and transforms them on the shuffleboard //creates tabs and transforms them on the shuffleboard
private void configureShuffleboard() { private void configureShuffleboard() {
ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab); ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
Shuffleboard.selectTab(OIConstants.kAutoTab);
autoTab.add("Auto Selection", autoChooser)
.withSize(2, 1)
.withPosition(0, 0)
.withWidget(BuiltInWidgets.kComboBoxChooser);
sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition) sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
.withSize(2, 1) .withSize(2, 1)
.withPosition(0, 0) .withPosition(0, 0)
.withWidget(BuiltInWidgets.kTextView); .withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition) sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
.withSize(2, 1) .withSize(2, 1)
.withPosition(2, 0) .withPosition(2, 0)
.withWidget(BuiltInWidgets.kTextView); .withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
.withSize(2, 1)
.withPosition(2, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
.withSize(2, 1)
.withPosition(0, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
.withSize(1, 1)
.withPosition(4, 0)
.withWidget(BuiltInWidgets.kBooleanBox);
sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
.withSize(1, 1)
.withPosition(4, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
.withSize(1, 1)
.withPosition(5, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
.withSize(1, 1)
.withPosition(5, 0)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
sensorTab.addDouble("velocity", drivetrain::getVelocity);
sensorTab.addDouble("heading", drivetrain::getHeading);
//sensorTab.add("odometry", drivetrain::getPose);
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
.withSize(1,1).withPosition(1,1);
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
.withSize(1,1).withPosition(2,1);
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
.withSize(1,1).withPosition(3,1);
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
.withSize(1,1).withPosition(4,1);
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
.withSize(1,1).withPosition(5,1);
apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
.withSize(1,1).withPosition(6,1);
apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
.withSize(1,1).withPosition(1,2);
apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
.withSize(1,1).withPosition(2,2);
apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
.withSize(1,1).withPosition(3,2);
apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
.withSize(1,1).withPosition(4,2);
apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
.withSize(1,1).withPosition(5,2);
apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
.withSize(1,1).withPosition(6,2);
apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
.withSize(2,1).withPosition(4,4);
apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
.withSize(2, 1).withPosition(4, 2);
apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
.withSize(2, 1).withPosition(6, 2);
} }
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
return autoChooser.getSelected(); return new PrintCommand("NO AUTO DEFINED");
} }
/** //teleop routines
* Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
* @return Moves the elevator and arm, then intakes coral
*/
@SuppressWarnings("unused")
private Command coralIntakeRoutine() { private Command coralIntakeRoutine() {
return moveManipulator( return moveManipulator(
ElevatorConstants.kCoralIntakePosition, ElevatorConstants.kElevatorCoralIntakePosition,
ManipulatorPivotConstants.kCoralIntakePosition ArmConstants.kArmCoralIntakePosition
) )
.andThen(manipulator.runUntilCollected(() -> .5)); .andThen(manipulator.runUntilCollected(1, true));
} }
/**
* Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
*
* @param l2 Is the algae on L2? (True = L2, False = L3)
* @return Moves the elevator and arm then intakes algae
*/
@SuppressWarnings("unused")
private Command algaeIntakeRoutine(boolean l2) { private Command algaeIntakeRoutine(boolean l2) {
return moveManipulator( return moveManipulator(
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition,
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
) )
.andThen(manipulator.runUntilCollected(() -> 1)); .andThen(manipulator.runUntilCollected(1, false));
} }
/**
* Moves the elevator and arm in different order based on target positions
*
* @param elevatorPosition The target position of the elevator
* @param armPosition The target rotation of the arm
* @return Moves the elevator and arm to the setpoints using the most efficient path
*/
private Command moveManipulator(double elevatorPosition, double armPosition) { private Command moveManipulator(double elevatorPosition, double armPosition) {
// If the elevator current and target positions are above the brace, or the arm current and target position is in // If the elevator current and target positions are above the brace, or the arm current and target position is in
// front of the brace, move together // front of the brace, move together
if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (arm.isMotionSafe() && arm.isMotionSafe(armPosition))) {
return moveManipulatorUtil(elevatorPosition, armPosition, false, false); return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first, // If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
// then the elevator, then the arm again // then the elevator, then the arm again
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) { } else if (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); .andThen(arm.goToSetpoint(armPosition, 2));
// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm // If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) { } else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, armPosition, true, true); return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
// If the arm is behind the brace, move the arm first, then the elevator // If the arm is behind the brace, move the arm first, then the elevator
} else if (!manipulatorPivot.isMotionSafe()) { } else if (!arm.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, armPosition, false, true); return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
// Catch all command that's safe regardless of arm and elevator positions // Catch all command that's safe regardless of arm and elevator positions
} else { } else {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); .andThen(arm.goToSetpoint(armPosition, 2));
} }
} }
/**
* Moves the elevator and arm in customizeable ways
*
* @param elevatorPosition The target elevator position
* @param armPosition The target arm position
* @param elevatorFirst Does the elevator move first? (True = Elevator first, False = Arm first)
* @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
* @return Moves the elevator and arm to the setpoints
*/
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) { private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
armPosition = MathUtil.clamp(
armPosition,
0,
ManipulatorPivotConstants.kRotationLimit
);
}*/
return Commands.either( return Commands.either(
Commands.either( Commands.either(
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
() -> sequential () -> sequential
), ),
Commands.either( Commands.either(
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)),
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)),
() -> sequential () -> sequential
), ),
() -> elevatorFirst () -> elevatorFirst
); );
} }
@SuppressWarnings("unused")
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
if(!isL4){
return Commands.sequence(
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition),
manipulatorPivot.goToSetpoint(() -> armPosition));
}else{
return Commands.sequence(
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
}
}
/**
* Moves the arm and elevator in a safe way.
*
* @param elevatorPosition The target position of the elevator
* @param armPosition The target rotation of the arm
* @return Moves the elevator and arm to the setpoints
*/
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
elevator.maintainPosition());*/
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
.raceWith(elevator.maintainPosition()));
}
private Command moveWithAlgae(double elevatorPosition, double armPosition) {
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
elevator.maintainPosition());*/
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
.raceWith(elevator.maintainPosition()));
}
private Command shootAlgae(){
return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition()));
}
@SuppressWarnings("unused")
private Command startingConfig() {
return moveManipulatorUtil(0, 0, false, true)
.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
}
} }

25
src/main/java/frc/robot/constants/ArmConstants.java Normal file
View File

@ -0,0 +1,25 @@
package frc.robot.constants;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
public class ArmConstants {
public static final int kArmMotorID = 0;
public static final int kCANcoderID = 0;
public static final double kEncoderConversionFactor = 0;
public static final double kArmMaxVelocity = 0;
public static final double kArmCoralIntakePosition = 0;
public static final double kArmL1Position = 0;
public static final double kArmL2Position = 0;
public static final double kArmL3Position = 0;
public static final double kArmL4Position = 0;
public static final double kArmL2AlgaePosition = 0;
public static final double kArmL3AlgaePosition = 0;
public static final double kArmSafeStowPosition = 0;
public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
}

42
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -1,50 +1,18 @@
package frc.robot.constants; package frc.robot.constants;
import java.io.IOException;
import org.json.simple.parser.ParseException;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants { public class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 5; public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 4; public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5; public static final double kPXController = 1;
public static final double kPYController = 1;
public static final double kPXYController = 3.5; public static final double kPThetaController = 1;
public static final double kPThetaController = 5;
public static final double kAlignPXYController = 2;
public static final double kAlignPThetaController = 5;
// Constraint for the motion profiled robot angle controller // Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
// TODO This is a constant being managed like a static rewriteable variable
public static RobotConfig kRobotConfig;
public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
new PIDConstants(kPXYController, 0, 0),
new PIDConstants(kPThetaController, 0, 0)
);
static {
try {
kRobotConfig = RobotConfig.fromGUISettings();
} catch (IOException | ParseException e) {
System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
e.printStackTrace();
}
}
} }

9
src/main/java/frc/robot/constants/ClimberPivotConstants.java
View File

@ -1,9 +1,7 @@
package frc.robot.constants; package frc.robot.constants;
import com.revrobotics.spark.config.SparkMaxConfig;
public class ClimberPivotConstants { public class ClimberPivotConstants {
public static final int kPivotMotorID = 10; public static final int kPivotMotorID = 0;
public static final int kClimberLimitSwitchID = 0; public static final int kClimberLimitSwitchID = 0;
@ -12,9 +10,4 @@ public class ClimberPivotConstants {
public static final double kPIDControllerP = 0; public static final double kPIDControllerP = 0;
public static final double kPIDControllerI = 0; public static final double kPIDControllerI = 0;
public static final double kPIDControllerD = 0; public static final double kPIDControllerD = 0;
public static final double kClimberClimbPosition = 0;
public static final double kClimberStartingPosition = 0;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
} }

15
src/main/java/frc/robot/constants/ClimberRollersConstants.java
View File

@ -1,18 +1,5 @@
package frc.robot.constants; package frc.robot.constants;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
public class ClimberRollersConstants { public class ClimberRollersConstants {
public static final int kRollerMotorID = 9; public static final int kRollerMotorID = 0;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static {
motorConfig
.smartCurrentLimit(40)
.idleMode(IdleMode.kBrake)
.inverted(true);
}
} }

45
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -13,26 +13,19 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class DrivetrainConstants { public class DrivetrainConstants {
// Driving Parameters - Note that these are not the maximum capable speeds of // Driving Parameters - Note that these are not the maximum capable speeds of
// the robot, rather the allowed maximum speeds // the robot, rather the allowed maximum speeds
public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75; public static final double kMaxSpeedMetersPerSecond = 4.8;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
// Chassis configuration // Chassis configuration
public static final double kTrackWidth = Units.inchesToMeters(24.5); public static final double kTrackWidth = Units.inchesToMeters(26.5);
// Distance between centers of right and left wheels on robot // Distance between centers of right and left wheels on robot
public static final double kWheelBase = Units.inchesToMeters(24.5); public static final double kWheelBase = Units.inchesToMeters(26.5);
// Angular offsets of the modules relative to the chassis in radians // Angular offsets of the modules relative to the chassis in radians
/* public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
public static final double kFrontLeftChassisAngularOffset = Math.PI; public static final double kFrontRightChassisAngularOffset = 0;
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2; public static final double kBackLeftChassisAngularOffset = Math.PI;
public static final double kBackLeftChassisAngularOffset = Math.PI / 2; public static final double kBackRightChassisAngularOffset = Math.PI / 2;
public static final double kBackRightChassisAngularOffset = 0;
*/
public static final double kFrontLeftChassisAngularOffset = Math.PI;
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
public static final double kBackRightChassisAngularOffset = 0;
// 1, 7, 10 is the default for these three values // 1, 7, 10 is the default for these three values
public static final double kSysIDDrivingRampRate = 1; public static final double kSysIDDrivingRampRate = 1;
@ -45,23 +38,17 @@ public class DrivetrainConstants {
public static final double kSysIDTurningTimeout = 10; public static final double kSysIDTurningTimeout = 10;
// SPARK MAX CAN IDs // SPARK MAX CAN IDs
public static final int kFrontLeftDrivingCanId = 0; public static final int kFrontLeftDrivingCanId = 11;
public static final int kRearLeftDrivingCanId = 2; public static final int kRearLeftDrivingCanId = 13;
public static final int kFrontRightDrivingCanId = 1; public static final int kFrontRightDrivingCanId = 15;
public static final int kRearRightDrivingCanId = 3; public static final int kRearRightDrivingCanId = 17;
public static final int kFrontLeftTurningCanId = 2; public static final int kFrontLeftTurningCanId = 10;
public static final int kRearLeftTurningCanId = 4; public static final int kRearLeftTurningCanId = 12;
public static final int kFrontRightTurningCanId = 7; public static final int kFrontRightTurningCanId = 14;
public static final int kRearRightTurningCanId = 5; public static final int kRearRightTurningCanId = 16;
public static final boolean kGyroReversed = true;
public static final double kHeadingP = 0.1;
public static final double kXTranslationP = 0.5;
public static final double kYTranslationP = 0.5;
public static final boolean kGyroReversed = false;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

73
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -11,56 +11,42 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class ElevatorConstants { public class ElevatorConstants {
public static final int kElevatorMotor1ID = 8; public static final int kElevatorMotor1ID = 0;
public static final int kElevatorMotor2ID = 6; public static final int kElevatorMotor2ID = 0;
public static final int kTopLimitSwitchID = 0;
public static final int kBottomLimitSwitchID = 0; public static final int kBottomLimitSwitchID = 0;
// 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position public static final double kEncoderConversionFactor = 0;
public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
public static final int kCurrentLimit = 40; public static final int kMotorAmpsMax = 0;
public static final double kUpControllerP = 5.6;//7; // public static final double kPositionControllerP = 0;
public static final double kUpControllerI = 0; public static final double kPositionControllerI = 0;
public static final double kUpControllerD = 0.28;//0.28 public static final double kPositionControllerD = 0;
public static final double kDownControllerP = 5.6;//7; // public static final double kVelocityControllerP = 0;
public static final double kDownControllerI = 0; public static final double kVelocityControllerI = 0;
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35 public static final double kVelocityControllerD = 0;
public static final double kMaintainP = 3; public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0;
public static final double kFeedForwardV = 0;
public static final double kAllowedError = 1; public static final double kElevatorMaxVelocity = 0;
public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */ public static final double kElevatorCoralIntakePosition = 0;
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6 public static final double kElevatorL1Position = 0;
public static final double kFeedForwardV = 0.12; // calculated value 0.12 public static final double kElevatorL2Position = 0;
public static final double kElevatorL3Position = 0;
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s public static final double kElevatorL4Position = 0;
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kElevatorL2AlgaePosition = 0;
public static final double kElevatorL3AlgaePosition = 0;
public static final double kCoralIntakePosition = 0; public static final double kElevatorBracePosition = 0;
public static final double kL1Position = 17;
public static final double kL2Position = 11;
public static final double kL3Position = 27;
public static final double kL4Position = 50.5;
public static final double kL4TransitionPosition = 40.0;
public static final double kL2AlgaePosition = 23.0;
public static final double kL3AlgaePosition = 39.0;
public static final double kProcessorPosition = 4.0;
/**The position of the top of the elevator brace */
public static final double kBracePosition = 0;
public static final double kMaxHeight = 51.0; //actual is 51
public static final double kVoltageLimit = 7;
public static final double kVoltageLimitAlgae = 9;
// 1, 7, 10 are the defaults for these, change as necessary // 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = .25; public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 3; public static final double kSysIDStepVolts = 7;
public static final double kSysIDTimeout = 10; public static final double kSysIDTimeout = 10;
public static final IdleMode kIdleMode = IdleMode.kBrake; public static final IdleMode kIdleMode = IdleMode.kBrake;
@ -77,11 +63,10 @@ public class ElevatorConstants {
static { static {
motorConfig motorConfig
.smartCurrentLimit(kCurrentLimit) .smartCurrentLimit(kMotorAmpsMax)
.idleMode(kIdleMode) .idleMode(kIdleMode);
.inverted(true);
motorConfig.encoder motorConfig.encoder
.positionConversionFactor(kEncoderPositionConversionFactor) .positionConversionFactor(kEncoderConversionFactor)
.velocityConversionFactor(kEncoderVelocityConversionFactor); .velocityConversionFactor(kEncoderConversionFactor / 60.0);
} }
} }

13
src/main/java/frc/robot/constants/IndexerConstants.java
View File

@ -1,15 +1,6 @@
package frc.robot.constants; package frc.robot.constants;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
public class IndexerConstants { public class IndexerConstants {
public static final int kIndexerMotorID = 16; public static final int kIndexerMotorID = 0;
public static final int kIndexerBeamBreakID = 0;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static{
motorConfig.smartCurrentLimit(30)
.idleMode(IdleMode.kBrake);
};
} }

15
src/main/java/frc/robot/constants/ManipulatorConstants.java
View File

@ -1,16 +1,7 @@
package frc.robot.constants; package frc.robot.constants;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
public class ManipulatorConstants { public class ManipulatorConstants {
public static final int kManipulatorMotorID = 12; public static final int kManipulatorMotorID = 0;
public static final int kCoralBeamBreakID = 2; public static final int kCoralBeamBreakID = 0;
public static final int kAlgaeBeamBreakID = 0;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static{
motorConfig.smartCurrentLimit(40)
.idleMode(IdleMode.kBrake);
};
} }

86
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@ -1,86 +0,0 @@
package frc.robot.constants;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class ManipulatorPivotConstants {
public static final int kPivotMotorID = 1;
public static final int kMotorCurrentMax = 40;
public static final double kPivotConversion = 2 * Math.PI;
public static final double kPivotMaxVelocity = 2 * Math.PI;
public static final double kPositionalP = 4.5;
public static final double kPositionalI = 0;
public static final double kPositionalD = 0;
public static final double kPositionalTolerance = Units.degreesToRadians(3);
public static final double kAlgaeP = 7;
public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
public static final double kFeedForwardV = 0.68; //calculated value 0.68
public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90);
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
public static final double kEncoderOffset = 0.04500000178813934;
public static final double kStartingPosition = Units.degreesToRadians(90);
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
public static final double kL1Position = Units.degreesToRadians(246);
public static final double kL2Position = Units.degreesToRadians(22.0+90);
public static final double kL3Position = Units.degreesToRadians(22.0+90);
public static final double kL4Position = Units.degreesToRadians(45.0+90);
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
public static final double kNetPosition = Units.degreesToRadians(175.0+90);
/**The closest position to the elevator brace without hitting it */
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
public static final double kBargeShotPosition = Units.degreesToRadians(222);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 7;
public static final double kSysIDTimeout = 10;
public static final IdleMode kIdleMode = IdleMode.kBrake;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
public static final SysIdRoutine.Config kSysIDConfig = new Config(
Volts.of(kSysIDRampRate).per(Second),
Volts.of(kSysIDStepVolts),
Seconds.of(kSysIDTimeout)
);
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static {
motorConfig
.smartCurrentLimit(kMotorCurrentMax)
.idleMode(kIdleMode)
.inverted(true);
motorConfig.absoluteEncoder
.positionConversionFactor(kPivotConversion)
.inverted(false)
.zeroOffset(kEncoderOffset);
}
}

84
src/main/java/frc/robot/constants/ModuleConstants.java
View File

@ -2,13 +2,6 @@ package frc.robot.constants;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.ctre.phoenix6.configs.AudioConfigs;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkMaxConfig;
public class ModuleConstants { public class ModuleConstants {
@ -23,83 +16,56 @@ public class ModuleConstants {
public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
// 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
// teeth on the bevel pinion // teeth on the bevel pinion
public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15); public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
/ kDrivingMotorReduction; / kDrivingMotorReduction;
public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction; public static final int kDriveMotorCurrentLimit = 40;
public static final double kTurningFactor = 2 * Math.PI;
public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
public static final double kDriveP = .04;
public static final double kDriveI = 0;
public static final double kDriveD = 0;
public static final double kDriveS = 0;
public static final double kDriveV = kDrivingVelocityFeedForward;
public static final double kDriveA = 0;
public static final double kTurnP = 1;
public static final double kTurnI = 0;
public static final double kTurnD = 0;
public static final int kDriveMotorStatorCurrentLimit = 100;
public static final int kDriveMotorSupplyCurrentLimit = 65;
public static final int kTurnMotorCurrentLimit = 20; public static final int kTurnMotorCurrentLimit = 20;
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
public static final SparkMaxConfig turningConfig = new SparkMaxConfig(); public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
public static final AudioConfigs kAudioConfig = new AudioConfigs();
public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
static { static {
kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction; // Use module constants to calculate conversion factors and feed forward gain.
double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
double turningFactor = 2 * Math.PI;
double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true; drivingConfig
kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true; .idleMode(IdleMode.kBrake)
kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit; .smartCurrentLimit(kDriveMotorCurrentLimit);
kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit; drivingConfig.encoder
.positionConversionFactor(drivingFactor) // meters
kDriveMotorConfig.Inverted = kDriveInversionState; .velocityConversionFactor(drivingFactor / 60.0); // meters per second
kDriveMotorConfig.NeutralMode = kDriveIdleMode; drivingConfig.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
kAudioConfig.AllowMusicDurDisable = true; // These are example gains you may need to them for your own robot!
.pid(0.04, 0, 0)
kDriveSlot0Config.kP = kDriveP; .velocityFF(drivingVelocityFeedForward)
kDriveSlot0Config.kI = kDriveI; .outputRange(-1, 1);
kDriveSlot0Config.kD = kDriveD;
kDriveSlot0Config.kS = kDriveS;
kDriveSlot0Config.kV = kDriveV;
kDriveSlot0Config.kA = kDriveA;
turningConfig turningConfig
.idleMode(kTurnIdleMode) .idleMode(IdleMode.kBrake)
.smartCurrentLimit(kTurnMotorCurrentLimit); .smartCurrentLimit(20);
turningConfig.absoluteEncoder turningConfig.absoluteEncoder
// Invert the turning encoder, since the output shaft rotates in the opposite // Invert the turning encoder, since the output shaft rotates in the opposite
// direction of the steering motor in the MAXSwerve Module. // direction of the steering motor in the MAXSwerve Module.
.inverted(true) .inverted(true)
.positionConversionFactor(kTurningFactor) // radians .positionConversionFactor(turningFactor) // radians
.velocityConversionFactor(kTurningFactor / 60.0); // radians per second .velocityConversionFactor(turningFactor / 60.0); // radians per second
turningConfig.closedLoop turningConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
// These are example gains you may need to them for your own robot! // These are example gains you may need to them for your own robot!
.pid(kTurnP, kTurnI, kTurnD) .pid(1, 0, 0)
.outputRange(-1, 1) .outputRange(-1, 1)
// Enable PID wrap around for the turning motor. This will allow the PID // Enable PID wrap around for the turning motor. This will allow the PID
// controller to go through 0 to get to the setpoint i.e. going from 350 degrees // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
// to 10 degrees will go through 0 rather than the other direction which is a // to 10 degrees will go through 0 rather than the other direction which is a
// longer route. // longer route.
.positionWrappingEnabled(true) .positionWrappingEnabled(true)
.positionWrappingInputRange(0, kTurningFactor); .positionWrappingInputRange(0, turningFactor);
} }
} }

2
src/main/java/frc/robot/constants/NeoMotorConstants.java
View File

@ -1,5 +1,5 @@
package frc.robot.constants; package frc.robot.constants;
public class NeoMotorConstants { public class NeoMotorConstants {
public static final double kFreeSpeedRpm = 6000; //for kraken not neo public static final double kFreeSpeedRpm = 5676;
} }

6
src/main/java/frc/robot/constants/OIConstants.java
View File

@ -4,11 +4,7 @@ public class OIConstants {
public static final int kDriverControllerPort = 0; public static final int kDriverControllerPort = 0;
public static final int kOperatorControllerPort = 1; public static final int kOperatorControllerPort = 1;
public static final double kDriveDeadband = Math.pow(0.05, 3); public static final double kDriveDeadband = 0.05;
public static final String kAutoTab = "Auto Tab";
public static final String kSensorsTab = "SensorsTab"; public static final String kSensorsTab = "SensorsTab";
public static final String kApriltagTab = "Apriltag Tab";
public static final double kJoystickExponential = 3;
} }

90
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -1,90 +0,0 @@
package frc.robot.constants;
public class VisionConstants {
// global coordinate map of all tags. index is the tag id.
// Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
// This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
public static final double[][] globalTagCoords = {{},
{656.98, 24.73, 58.50, 126.0, 0},
{656.98, 291.90, 58.50, 234.0, 0},
{452.4, 316.21, 51.25, 270, 0},
{365.2, 241.44, 73.54, 0, 30},
{365.2, 75.19, 73.54, 0, 30},
{530.49, 129.97, 12.13, 300, 0},
{546.87, 158.3, 12.13, 0, 0},
{530.49, 186.63, 12.13, 60, 0},
{497.77, 186.63, 12.13, 120, 0},
{481.39, 158.3, 12.13, 180, 0},
{497.77, 129.97, 12.13, 240, 0},
{33.9, 24.73, 58.5, 54, 0},
{33.9, 291.9, 58.5, 306, 0},
{325.68, 241.44, 73.54, 180, 30},
{325.68, 75.19, 73.54, 180, 30},
{238.49, 0.42, 51.25, 90, 0},
{160.39, 129.97, 12.13, 240, 0},
{144.00, 158.3, 12.13, 180, 0},
{160.39, 186.63, 12.13, 120, 0},
{193.1, 186.63, 12.13, 60, 0},
{209.49, 158.3, 12.13, 0, 0},
{193.1, 129.97, 12.13, 300, 0},
};
//map of coral placing setpoints based on the tag that is on the same reef face
// and the on the left or right branch of that side of the reef
// <tag_number, {left_x, left_y, right_x, right_y}>
public static final double[][] reefSetpointsMap = {
{},
{},
{},
{},
{},
{},
{13.570, 2.816, 13.858, 2.970},//6
{14.373, 3.862, 14.385, 4.194},
{13.858, 5.032, 13.558, 5.227},
{12.575, 5.227, 12.287, 5.056},
{11.772, 4.169, 11.772, 3.845},
{12.287, 2.982, 12.587, 2.826},//11
{},
{},
{},
{},
{},
{3.703, 3.975, 3.982, 2.806},
{3.183, 4.191, 3.183, 3.857},
{3.986, 5.24, 3.701, 5.076},
{5.275, 5.075, 4.891, 5.284},//4.991, 5.246},
{5.789, 3.862, 5.789, 4.194},
{4.993, 2.816, 5.272, 2.996}
};
public static final double[][] algaeSetpointsMap = {
{},
{},
{},
{},
{},
{},
{13.906, 2.658},//6
{14.661, 4.013},
{13.834, 5.428},
{12.263, 5.452},
{11.412, 4.025},
{12.191, 2.574},//11
{},
{},
{},
{},
{},
{3.649, 2.558},//17
{2.776, 4.005},
{3.644, 5.514},
{5.296, 5.522},//4.991, 5.246},
{6.225, 4.008},
{5.322, 2.511}//22
};
public static final double latencyFudge = 0.0;
}

94
src/main/java/frc/robot/subsystems/Arm.java Normal file
View File

@ -0,0 +1,94 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ArmConstants;
public class Arm extends SubsystemBase {
private SparkMax armMotor;
private CANcoder canCoder;
private PIDController positionController;
private PIDController velocityController;
private ArmFeedforward feedForward;
public Arm() {
armMotor = new SparkMax(
ArmConstants.kArmMotorID,
MotorType.kBrushless
);
canCoder = new CANcoder(ArmConstants.kCANcoderID);
}
/**
* Returns whether or not the motion is safe relative to the encoder's current position
* and the arm safe stow position
*
* @return Is the motion safe
*/
public boolean isMotionSafe() {
return isMotionSafe(getEncoderPosition());
}
/**
* Returns whether or not the motion is safe relative to some target position and the
* arm safe stow position
*
* @param motionTarget The target position to determine the safety of
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ArmConstants.kArmSafeStowPosition;
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runArm(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
double voltsOut = velocityController.calculate(
getEncoderVelocity(),
realSpeedTarget
) + feedForward.calculate(
getEncoderPosition(),
getEncoderVelocity()
);
armMotor.setVoltage(voltsOut);
});
}
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = positionController.calculate(
getEncoderPosition(),
setpoint
) + feedForward.calculate(
getEncoderPosition(),
getEncoderVelocity()
);
armMotor.setVoltage(voltsOut);
}).until(positionController::atSetpoint).withTimeout(timeout);
}
public double getEncoderPosition() {
return Units.rotationsToRadians(canCoder.getPosition().getValueAsDouble());
}
public double getEncoderVelocity() {
return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
}
}

50
src/main/java/frc/robot/subsystems/ClimberPivot.java
View File

@ -1,13 +1,11 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.revrobotics.RelativeEncoder; import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ClimberPivotConstants;
@ -17,41 +15,45 @@ public class ClimberPivot extends SubsystemBase {
private RelativeEncoder neoEncoder; private RelativeEncoder neoEncoder;
private DigitalInput cageLimitSwitch;
private PIDController pidController;
public ClimberPivot() { public ClimberPivot() {
pivotMotor = new SparkMax( pivotMotor = new SparkMax(
ClimberPivotConstants.kPivotMotorID, ClimberPivotConstants.kPivotMotorID,
MotorType.kBrushless MotorType.kBrushless
); );
pivotMotor.configure(
ClimberPivotConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
neoEncoder = pivotMotor.getEncoder(); neoEncoder = pivotMotor.getEncoder();
cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
pidController = new PIDController(
ClimberPivotConstants.kPIDControllerP,
ClimberPivotConstants.kPIDControllerI,
ClimberPivotConstants.kPIDControllerD
);
} }
public Command runPivot(DoubleSupplier speed) { public Command runPivot(double speed) {
return run(() -> { return run(() -> {
pivotMotor.set(speed.getAsDouble()); pivotMotor.set(speed);
}); });
} }
/** public Command goToAngle(double setpoint, double timeout) {
* Runs the climber until it is at setpoint
*
* @param speed The speed at which the pivot runs
* @param setpoint The target position of the climber
* @return Sets the motor speed until at the target position
*/
public Command climb(double setpoint, double speed) {
return run(() -> { return run(() -> {
pivotMotor.set(speed); pivotMotor.set(
}).until(() -> neoEncoder.getPosition() >= setpoint); pidController.calculate(
neoEncoder.getPosition(),
setpoint
)
);
}).withTimeout(timeout);
} }
public double getEncoderPosition() { public boolean getCageLimitSwitch() {
return neoEncoder.getPosition(); return cageLimitSwitch.get();
} }
} }

21
src/main/java/frc/robot/subsystems/ClimberRollers.java
View File

@ -1,17 +1,12 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ClimberRollersConstants; import frc.robot.constants.ClimberRollersConstants;
//TODO Figure out a way to detect if we're at the top of the cage
public class ClimberRollers extends SubsystemBase { public class ClimberRollers extends SubsystemBase {
private SparkMax rollerMotor; private SparkMax rollerMotor;
@ -20,23 +15,11 @@ public class ClimberRollers extends SubsystemBase {
ClimberRollersConstants.kRollerMotorID, ClimberRollersConstants.kRollerMotorID,
MotorType.kBrushless MotorType.kBrushless
); );
rollerMotor.configure(
ClimberRollersConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
} }
/** public Command runRoller(double speed) {
* Runs the rollers at a set speed
*
* @param speed The speed in which the roller runs
* @return Runs the rollers at a set speed
*/
public Command runRoller(DoubleSupplier speed) {
return run(() -> { return run(() -> {
rollerMotor.set(speed.getAsDouble()); rollerMotor.set(speed);
}); });
} }
} }

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

@ -4,45 +4,23 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.io.File;
import java.util.Optional;
import java.util.function.BooleanSupplier; import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier; import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Orchestra;
import com.pathplanner.lib.auto.AutoBuilder;
import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants; import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants;
import edu.wpi.first.math.geometry.Pose3d;
public class Drivetrain extends SubsystemBase { public class Drivetrain extends SubsystemBase {
// Create MAXSwerveModules // Create MAXSwerveModules
@ -52,26 +30,10 @@ public class Drivetrain extends SubsystemBase {
protected MAXSwerveModule m_rearRight; protected MAXSwerveModule m_rearRight;
// The gyro sensor // The gyro sensor
private AHRS gyro; private ADIS16470_IMU m_gyro;
// Odometry class for tracking robot pose // Odometry class for tracking robot pose
private SwerveDrivePoseEstimator m_estimator; private SwerveDriveOdometry m_odometry;
private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
public Orchestra m_orchestra = new Orchestra();
private Timer musicTimer = new Timer();
private ProfiledPIDController pidHeading;
private PIDController pidTranslationX;
private PIDController pidTranslationY;
private HolonomicDriveController driveController;
public Vision vision;
public Pose2d orangePose2d;
public Pose2d blackPose2d;
/** Creates a new DriveSubsystem. */ /** Creates a new DriveSubsystem. */
public Drivetrain() { public Drivetrain() {
@ -99,169 +61,30 @@ public class Drivetrain extends SubsystemBase {
DrivetrainConstants.kBackRightChassisAngularOffset DrivetrainConstants.kBackRightChassisAngularOffset
); );
gyro = new AHRS(NavXComType.kMXP_SPI); m_gyro = new ADIS16470_IMU();
m_estimator = new SwerveDrivePoseEstimator( m_odometry = new SwerveDriveOdometry(
DrivetrainConstants.kDriveKinematics, DrivetrainConstants.kDriveKinematics,
Rotation2d.fromDegrees(getGyroValue()), Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
new Pose2d(),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
);
pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
pidHeading.setTolerance(Units.degreesToRadians(3));
pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
AutoBuilder.configure(
this::getPose,
this::resetOdometry,
this::getCurrentChassisSpeeds,
this::driveWithChassisSpeeds,
AutoConstants.kPPDriveController,
AutoConstants.kRobotConfig,
() -> {
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this
);
m_orchestra.loadMusic(Filesystem.getDeployDirectory()
.toPath()
.resolve("Orchestra" + File.separator + "doomE1M1.chrp")
.toString());
// Add a single device to the orchestra
m_orchestra.addInstrument(m_frontLeft.getDrivingMotor(), 0);
m_orchestra.addInstrument(m_frontRight.getDrivingMotor(), 1);
m_orchestra.addInstrument(m_rearLeft.getDrivingMotor(), 2);
m_orchestra.addInstrument(m_rearRight.getDrivingMotor(), 3);
m_orchestra.play();
musicTimer.reset();
musicTimer.start();
vision = new Vision();
orangePose2d = new Pose2d();
blackPose2d = new Pose2d();
}
@Override
public void periodic() {
// Update the odometry in the periodic block
m_estimator.update(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
m_frontLeft.getPosition(), m_frontLeft.getPosition(),
m_frontRight.getPosition(), m_frontRight.getPosition(),
m_rearLeft.getPosition(), m_rearLeft.getPosition(),
m_rearRight.getPosition() m_rearRight.getPosition()
}); });
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
} }
// if the detected tags match your alliances reef tags use their pose estimates @Override
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){ public void periodic() {
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer); // Update the odometry in the periodic block
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp()); m_odometry.update(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){ new SwerveModulePosition[] {
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer); m_frontLeft.getPosition(),
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp()); m_frontRight.getPosition(),
} m_rearLeft.getPosition(),
} m_rearRight.getPosition()
Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); });
Logger.recordOutput("orange dist", vision.getOrangeDist());
Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
Logger.recordOutput("orange FPS", vision.getOrangeFPS());
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
Logger.recordOutput("black dist", vision.getBlackDist());
Logger.recordOutput("black detected", vision.getBlackTagDetected());
Logger.recordOutput("black tag", vision.getBlackTagDetected());
Logger.recordOutput("black FPS", vision.getBlackFPS());
Logger.recordOutput("drive velocity", getVelocity());
Logger.recordOutput("closest tag", getClosestTag());
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>10){
if (m_orchestra.isPlaying()) {
m_orchestra.stop();
}
musicTimer.stop();
musicTimer.reset();
}
}
public ChassisSpeeds getCurrentChassisSpeeds() {
return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
m_frontLeft.getState(),
m_frontRight.getState(),
m_rearLeft.getState(),
m_rearRight.getState()
);
}
public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
setModuleStates(newStates);
} }
/** /**
@ -270,7 +93,7 @@ public class Drivetrain extends SubsystemBase {
* @return The pose. * @return The pose.
*/ */
public Pose2d getPose() { public Pose2d getPose() {
return m_estimator.getEstimatedPosition(); return m_odometry.getPoseMeters();
} }
/** /**
@ -279,9 +102,8 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry. * @param pose The pose to which to set the odometry.
*/ */
public void resetOdometry(Pose2d pose) { public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
m_estimator.resetPosition( Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
m_frontLeft.getPosition(), m_frontLeft.getPosition(),
m_frontRight.getPosition(), m_frontRight.getPosition(),
@ -295,20 +117,12 @@ public class Drivetrain extends SubsystemBase {
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot, public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
BooleanSupplier fieldRelative) { BooleanSupplier fieldRelative) {
return run(() -> { return run(() -> {
if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
drive( drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband), -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband), -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean() fieldRelative.getAsBoolean()
); );
}else{
drive(
MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean()
);}
}); });
} }
@ -323,24 +137,14 @@ public class Drivetrain extends SubsystemBase {
*/ */
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// Convert the commanded speeds into the correct units for the drivetrain // Convert the commanded speeds into the correct units for the drivetrain
double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
double xSpeedDelivered = 0; double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
double ySpeedDelivered = 0;
if(p != 0){
xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
}else{
xSpeedDelivered = 0;
ySpeedDelivered = 0;
}
double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed; double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians())) Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds( SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@ -356,71 +160,6 @@ public class Drivetrain extends SubsystemBase {
}); });
} }
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
return startRun(() -> {
pidTranslationX.reset();
pidTranslationY.reset();
pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
},
() -> {
ChassisSpeeds controlEffort = driveController.calculate(
m_estimator.getEstimatedPosition(),
new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(),
headingSetpoint.get()),
0,
headingSetpoint.get()
);
double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
controlEffort.vxMetersPerSecond *= mul;
controlEffort.vyMetersPerSecond *= mul;
}
driveWithChassisSpeeds(controlEffort);
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
headingSetpoint.get())));
});
}
public int getClosestTag(){
if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
int closestTag = 17;
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][0]), 2)
+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][1]), 2));
for(int i = 17; i <= 22; ++i){
double distance = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][0]), 2)
+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
if(distance < closestTagDist){
closestTag = i;
closestTagDist = distance;
}
}
return closestTag;
}else{
int closestTag = 6;
double closestTagDist = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][0]), 2)
+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][1]), 2));
for(int i = 6; i <= 11; ++i){
double distance = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters( VisionConstants.globalTagCoords[i][0]), 2)
+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
if(distance < closestTagDist){
closestTag = i;
closestTagDist = distance;
}
}
return closestTag;
}
}
/** /**
* Sets the wheels into an X formation to prevent movement. * Sets the wheels into an X formation to prevent movement.
*/ */
@ -453,17 +192,9 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.resetEncoders(); m_rearRight.resetEncoders();
} }
/** Zeroes the heading of the robot. /** Zeroes the heading of the robot. */
* @return */ public void zeroHeading() {
public Command zeroHeading() { m_gyro.reset();
return run(() -> {
gyro.reset();
m_estimator.resetRotation(new Rotation2d(0));
});
}
public double getGyroValue() {
return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
} }
/** /**
@ -472,11 +203,7 @@ public class Drivetrain extends SubsystemBase {
* @return the robot's heading in degrees, from -180 to 180 * @return the robot's heading in degrees, from -180 to 180
*/ */
public double getHeading() { public double getHeading() {
return m_estimator.getEstimatedPosition().getRotation().getDegrees(); return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
}
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
return gyroBuffer;
} }
/** /**
@ -485,20 +212,6 @@ public class Drivetrain extends SubsystemBase {
* @return The turn rate of the robot, in degrees per second * @return The turn rate of the robot, in degrees per second
*/ */
public double getTurnRate() { public double getTurnRate() {
return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
}
public double getVelocity(){
return m_frontLeft.getState().speedMetersPerSecond;
}
public Command resetToVision(){
return runOnce(() -> {
if(vision.getOrangeTagDetected()){
m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
}else if(vision.getBlackTagDetected()){
m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
}
});
} }
} }

298
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -2,15 +2,12 @@ package frc.robot.subsystems;
import java.util.function.DoubleSupplier; import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.RelativeEncoder; import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
@ -26,10 +23,8 @@ public class Elevator extends SubsystemBase {
private DigitalInput bottomLimitSwitch; private DigitalInput bottomLimitSwitch;
private PIDController pidControllerUp; private PIDController positionController;
private PIDController pidControllerDown; private PIDController velocityController;
private PIDController maintainPID;
private ElevatorFeedforward feedForward; private ElevatorFeedforward feedForward;
@ -62,27 +57,17 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kBottomLimitSwitchID ElevatorConstants.kBottomLimitSwitchID
); );
pidControllerDown = new PIDController( positionController = new PIDController(
ElevatorConstants.kDownControllerP, ElevatorConstants.kPositionControllerP,
ElevatorConstants.kDownControllerI, ElevatorConstants.kPositionControllerI,
ElevatorConstants.kDownControllerD ElevatorConstants.kPositionControllerD
); );
pidControllerDown.setSetpoint(0);
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError); velocityController = new PIDController(
ElevatorConstants.kVelocityControllerP,
pidControllerUp = new PIDController( ElevatorConstants.kVelocityControllerI,
ElevatorConstants.kUpControllerP, ElevatorConstants.kVelocityControllerD
ElevatorConstants.kUpControllerI,
ElevatorConstants.kUpControllerD
); );
pidControllerUp.setSetpoint(0);
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
feedForward = new ElevatorFeedforward( feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS, ElevatorConstants.kFeedForwardS,
@ -91,17 +76,6 @@ public class Elevator extends SubsystemBase {
); );
} }
@Override
public void periodic() {
if (!getBottomLimitSwitch()) {
encoder.setPosition(0);
}
Logger.recordOutput("elevator position", getEncoderPosition());
Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
}
/** /**
* Returns whether or not the motion is safe relative to the encoder's current position * Returns whether or not the motion is safe relative to the encoder's current position
* and the elevator brace position * and the elevator brace position
@ -120,247 +94,39 @@ public class Elevator extends SubsystemBase {
* @return Is the motion safe * @return Is the motion safe
*/ */
public boolean isMotionSafe(double motionTarget) { public boolean isMotionSafe(double motionTarget) {
return motionTarget > ElevatorConstants.kBracePosition; return motionTarget > ElevatorConstants.kElevatorBracePosition;
} }
/** //manual command that keeps ouput speed consistent no matter the direction
* A manual translation command that uses feed forward calculation to maintain position public Command runElevator(DoubleSupplier speed) {
*
* @param speed The speed at which the elevator translates
* @return Sets motor voltage to translate the elevator and maintain position
*/
public Command runManualElevator(DoubleSupplier speed) {
return run(() -> { return run(() -> {
double desired = speed.getAsDouble(); double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) { double voltsOut = velocityController.calculate(
elevatorMotor1.set( encoder.getVelocity(),
speed.getAsDouble() realSpeedTarget
); ) + feedForward.calculate(realSpeedTarget);
} else {
elevatorMotor1.setVoltage(feedForward.calculate(0)); elevatorMotor1.setVoltage(voltsOut);
}).until(bottomLimitSwitch::get);
} }
});
}
/** //go to setpoint command
* A command that will use the feed forward to hold up the elevator. public Command goToSetpoint(double setpoint, double timeout) {
* Used for feed forward tuning.
*
* @return Sets motor voltage based on feed forward calculation.
*/
public Command maintainPosition() {
return startRun(() -> {
maintainPID.reset();
maintainPID.setSetpoint(pidControllerUp.getSetpoint());
},
() -> {
double maintainOutput = maintainPID.calculate(getEncoderPosition());
if(!maintainPID.atSetpoint())
elevatorMotor1.setVoltage( MathUtil.clamp(
maintainOutput + feedForward.calculate(0), -2, 2)
);
else{
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}
/*
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
*/
});
}
public Command homeCommand(){
return run(() -> { return run(() -> {
elevatorMotor1.setVoltage(0.5); double voltsOut = positionController.calculate(
}) encoder.getPosition(),
.until(() -> elevatorMotor1.getOutputCurrent() > 5) setpoint
.andThen(run(() -> encoder.setPosition(0))); ) + feedForward.calculate(0);
elevatorMotor1.setVoltage(voltsOut);
}).until(
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
).withTimeout(timeout);
} }
/**
* Moves the elevator to a target destination (setpoint).
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(DoubleSupplier setpoint) {
if (setpoint.getAsDouble() == 0) {
return startRun(() -> {
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
);
else{
elevatorMotor1.setVoltage(
MathUtil.clamp(
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
.andThen(runManualElevator(() -> -.5)
.until(() -> encoder.getPosition() == 0));
} else {
return startRun(() -> {
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
);
else{
elevatorMotor1.setVoltage(
MathUtil.clamp(
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
}
}
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
if (setpoint.getAsDouble() == 0) {
return startRun(() -> {
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
else{
elevatorMotor1.setVoltage(
MathUtil.clamp(
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
.andThen(runManualElevator(() -> -.5)
.until(() -> encoder.getPosition() == 0));
} else {
return startRun(() -> {
pidControllerUp.reset();
pidControllerDown.reset();
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
},
() -> {
double upOutput = pidControllerUp.calculate(getEncoderPosition());
double downOutput = pidControllerDown.calculate(getEncoderPosition());
if(setpoint.getAsDouble()>encoder.getPosition())
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
else{
elevatorMotor1.setVoltage(
MathUtil.clamp(
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
}
}
/**
* Returns the current encoder position
*
* @return Current encoder position
*/
public double getEncoderPosition() { public double getEncoderPosition() {
return encoder.getPosition(); return encoder.getPosition();
} }
/**
* Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
*
* @return The value of bottomLimitSwitch
*/
public boolean getBottomLimitSwitch() {
return bottomLimitSwitch.get();
}
/**
* Returns the motor's output current
*
* @return Motor output current
*/
public double getMotor1() {
return elevatorMotor1.getAppliedOutput()*elevatorMotor1.getBusVoltage();
}
/**
* Returns the motor's output current
*
* @return Motor output current
*/
public double getMotor2() {
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
}
public double getPIDUpSetpoint() {
return pidControllerUp.getSetpoint();
}
public double getPIDUpError() {
return pidControllerUp.getError();
}
public double getPIDDownSetpoint() {
return pidControllerDown.getSetpoint();
}
public double getPIDDownError() {
return pidControllerDown.getError();
}
} }

36
src/main/java/frc/robot/subsystems/Indexer.java Normal file
View File

@ -0,0 +1,36 @@
package frc.robot.subsystems;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IndexerConstants;
public class Indexer extends SubsystemBase {
private SparkMax indexerMotor;
private DigitalInput indexerBeamBreak;
public Indexer() {
indexerMotor = new SparkMax(
IndexerConstants.kIndexerMotorID,
MotorType.kBrushless
);
indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID);
}
public Command runIndexer(double speed) {
return run(() -> {
indexerMotor.set(speed);
});
}
public Command indexCoral(double speed) {
return run(() -> {
indexerMotor.set(speed);
}).until(indexerBeamBreak::get);
}
}

50
src/main/java/frc/robot/subsystems/MAXSwerveModule.java
View File

@ -15,22 +15,21 @@ import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.AbsoluteEncoder; import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import frc.robot.constants.ModuleConstants; import frc.robot.constants.ModuleConstants;
public class MAXSwerveModule { public class MAXSwerveModule {
private final TalonFX m_drive; private final SparkMax m_drivingSpark;
private final SparkMax m_turningSpark; private final SparkMax m_turningSpark;
private final RelativeEncoder m_drivingEncoder;
private final AbsoluteEncoder m_turningEncoder; private final AbsoluteEncoder m_turningEncoder;
private final SparkClosedLoopController m_drivingClosedLoopController;
private final SparkClosedLoopController m_turningClosedLoopController; private final SparkClosedLoopController m_turningClosedLoopController;
private final VelocityVoltage driveVelocityRequest;
private double m_chassisAngularOffset = 0; private double m_chassisAngularOffset = 0;
private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
@ -41,30 +40,26 @@ public class MAXSwerveModule {
* Encoder. * Encoder.
*/ */
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
m_drive = new TalonFX(drivingCANId); m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
m_drivingEncoder = m_drivingSpark.getEncoder();
m_turningEncoder = m_turningSpark.getAbsoluteEncoder(); m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
m_turningClosedLoopController = m_turningSpark.getClosedLoopController(); m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
// Apply the respective configurations to the SPARKS. Reset parameters before // Apply the respective configurations to the SPARKS. Reset parameters before
// applying the configuration to bring the SPARK to a known good state. Persist // applying the configuration to bring the SPARK to a known good state. Persist
// the settings to the SPARK to avoid losing them on a power cycle. // the settings to the SPARK to avoid losing them on a power cycle.
m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig); m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig); PersistMode.kPersistParameters);
m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters, m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters); PersistMode.kPersistParameters);
m_chassisAngularOffset = chassisAngularOffset; m_chassisAngularOffset = chassisAngularOffset;
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition()); m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
m_drive.setPosition(0); m_drivingEncoder.setPosition(0);
} }
/** /**
@ -75,7 +70,7 @@ public class MAXSwerveModule {
public SwerveModuleState getState() { public SwerveModuleState getState() {
// Apply chassis angular offset to the encoder position to get the position // Apply chassis angular offset to the encoder position to get the position
// relative to the chassis. // relative to the chassis.
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, return new SwerveModuleState(m_drivingEncoder.getVelocity(),
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
} }
@ -87,7 +82,8 @@ public class MAXSwerveModule {
public SwerveModulePosition getPosition() { public SwerveModulePosition getPosition() {
// Apply chassis angular offset to the encoder position to get the position // Apply chassis angular offset to the encoder position to get the position
// relative to the chassis. // relative to the chassis.
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, return new SwerveModulePosition(
m_drivingEncoder.getPosition(),
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
} }
@ -106,21 +102,14 @@ public class MAXSwerveModule {
correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition())); correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
// Command driving and turning SPARKS towards their respective setpoints. // Command driving and turning SPARKS towards their respective setpoints.
m_drive.setControl( m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
driveVelocityRequest.withVelocity(
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
).withFeedForward(
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
)
);
m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition); m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
m_desiredState = desiredState; m_desiredState = desiredState;
} }
public void setVoltageDrive(double voltage){ public void setVoltageDrive(double voltage){
m_drive.setVoltage(voltage); m_drivingSpark.setVoltage(voltage);
} }
public void setVoltageTurn(double voltage) { public void setVoltageTurn(double voltage) {
@ -128,20 +117,15 @@ public class MAXSwerveModule {
} }
public double getVoltageDrive() { public double getVoltageDrive() {
return m_drive.get() * RobotController.getBatteryVoltage(); return m_drivingSpark.get() * RobotController.getBatteryVoltage();
} }
public double getVoltageTurn() { public double getVoltageTurn() {
return m_turningSpark.get() * RobotController.getBatteryVoltage(); return m_turningSpark.get() * RobotController.getBatteryVoltage();
} }
public TalonFX getDrivingMotor(){
return m_drive;
}
/** Zeroes all the SwerveModule encoders. */ /** Zeroes all the SwerveModule encoders. */
public void resetEncoders() { public void resetEncoders() {
m_drive.setPosition(0); m_drivingEncoder.setPosition(0);
} }
} }

119
src/main/java/frc/robot/subsystems/Manipulator.java
View File

@ -1,26 +1,18 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IndexerConstants;
import frc.robot.constants.ManipulatorConstants; import frc.robot.constants.ManipulatorConstants;
public class Manipulator extends SubsystemBase { public class Manipulator extends SubsystemBase {
private SparkMax manipulatorMotor; private SparkMax manipulatorMotor;
private SparkMax indexerMotor;
private DigitalInput coralBeamBreak; private DigitalInput coralBeamBreak;
private DigitalInput algaeBeamBreak;
public Manipulator() { public Manipulator() {
manipulatorMotor = new SparkMax( manipulatorMotor = new SparkMax(
@ -28,116 +20,19 @@ public class Manipulator extends SubsystemBase {
MotorType.kBrushless MotorType.kBrushless
); );
manipulatorMotor.configure(
ManipulatorConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
indexerMotor = new SparkMax(
IndexerConstants.kIndexerMotorID,
MotorType.kBrushless
);
indexerMotor.configure(
IndexerConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID); coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
} }
@Override public Command runManipulator(double speed) {
public void periodic() {
super.periodic();
Logger.recordOutput("coral beam break", getCoralBeamBreak());
}
/**
* The default command for the manipulator that either stops the manipulator or slowly
* runs the manipulator to retain the algae
*
* @return Returns a command that sets the speed of the motor
*/
public Command defaultCommand() {
return run(() -> { return run(() -> {
runUntilCollected(() -> 0.1); manipulatorMotor.set(speed);
}); });
} }
/** public Command runUntilCollected(double speed, boolean coral) {
* Runs the manipulator at a set speed with the direction based on the coral parameter
*
* @param speed The speed at which the manipulator runs
* @param coral Is the manipulator manipulating a coral? (True = Coral, False = Algae)
* @return Returns a command that sets the speed of the motor
*/
public Command runManipulator(DoubleSupplier speed, boolean coral) {
return run(() -> { return run(() -> {
manipulatorMotor.set( manipulatorMotor.set(coral ? speed : speed * -1);
coral ? speed.getAsDouble() : speed.getAsDouble() * -1 }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
);
indexerMotor.set(0);
});
}
/**
* Runs the manipulator until either the algae or coral beam break reads true
*
* @param speed The speed at which the manipulator is run
* @param coral Is the object a coral? (True = Coral, False = Algae)
* @return Returns a command that sets the speed of the motor
*/
public Command runUntilCollected(DoubleSupplier speed) {
return run(() -> {
manipulatorMotor.setVoltage(
speed.getAsDouble()*12
);
indexerMotor.set(1);
}).unless(() -> !coralBeamBreak.get())
.until(() -> !coralBeamBreak.get());
/*
return run(() -> {
if(getCoralBeamBreak()) {
manipulatorMotor.set(
speed.getAsDouble()
);
} else {
manipulatorMotor.set(
speed.getAsDouble()
);
}
*/
}
public Command retractCommand(DoubleSupplier retractSpeed){
return run(() -> {
manipulatorMotor.set(-retractSpeed.getAsDouble());
indexerMotor.set(0);
}
).until(() -> coralBeamBreak.get());
}
/**
* Runs the manipulator in a way that will bring the coral to a reliable holding position
*
* @return Returns a command that will position the coral to a known location
*/
public Command indexCoral() {
return run(() -> {
runUntilCollected(() -> 0.5)
.andThen(runManipulator(() -> .1, false))
.until(() -> getCoralBeamBreak());
});
}
public boolean getCoralBeamBreak() {
return coralBeamBreak.get();
} }
} }

238
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -1,238 +0,0 @@
package frc.robot.subsystems;
import com.revrobotics.spark.SparkMax;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ManipulatorPivotConstants;
public class ManipulatorPivot extends SubsystemBase {
protected SparkMax pivotMotor;
private SparkAbsoluteEncoder encoder;
private ArmFeedforward feedForward;
private PIDController pidController;
private PIDController algaePIDController;
public ManipulatorPivot() {
pivotMotor = new SparkMax(
ManipulatorPivotConstants.kPivotMotorID,
MotorType.kBrushless
);
pivotMotor.configure(
ManipulatorPivotConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
encoder = pivotMotor.getAbsoluteEncoder();
pidController = new PIDController(
ManipulatorPivotConstants.kPositionalP,
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD
);
pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
pidController.setSetpoint(0);
pidController.enableContinuousInput(0, 280);
algaePIDController = new PIDController(
ManipulatorPivotConstants.kAlgaeP,
0,
0);
algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
algaePIDController.setSetpoint(0);
algaePIDController.enableContinuousInput(0, 280);
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,
ManipulatorPivotConstants.kFeedForwardG,
ManipulatorPivotConstants.kFeedForwardV
);
}
@Override
public void periodic() {
super.periodic();
Logger.recordOutput("manipulator position", getEncoderPosition());
Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
}
/**
* Returns whether or not the motion is safe relative to the encoder's current position
* and the arm safe stow position
*
* @return Is the motion safe
*/
public boolean isMotionSafe() {
return isMotionSafe(getEncoderPosition());
}
/**
* Returns whether or not the motion is safe relative to some target position and the
* arm safe stow position
*
* @param motionTarget The target position to determine the safety of
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ManipulatorPivotConstants.kPivotSafeStowPosition;
}
/**
* Manual ManipulatorPivot command that sets the motor based on speed
*
* @param speed The speed to set the motor
* @return A command that sets the motor speed
*/
public Command runManualPivot(DoubleSupplier speed) {
return run(() -> {
pivotMotor.set(speed.getAsDouble());
});
}
/**
* Moves the arm to a target destination (setpoint)
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(DoubleSupplier setpoint) {
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
} else {
pivotMotor.setVoltage(
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
*/
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}).until(() -> pidController.atSetpoint());
}
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
return startRun(() -> {
algaePIDController.setSetpoint(setpoint.getAsDouble());
algaePIDController.reset();
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
} else {
pivotMotor.setVoltage(
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
*/
pivotMotor.setVoltage(
algaePIDController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}).until(() -> algaePIDController.atSetpoint());
}
public Command maintainPosition() {
return startRun(() -> {
pidController.reset();
},
() -> {
/*
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
} else {
pivotMotor.setVoltage(
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
*/
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
pidController.getSetpoint()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
});
}
/**
* Returns the encoder's position in radians
*
* @return Encoder's position in radians
*/
public double getEncoderPosition() {
return Units.radiansToDegrees( encoder.getPosition());
}
/**
* Returns the encoder's velocity in radians per second
*
* @return Encoder's velocity in radians per second
*/
public double getEncoderVelocity() {
return Units.radiansToDegrees(encoder.getVelocity());
}
public double getCGPosition(){
return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
}
public double getPivotOutput(){
return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
}
}

187
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -1,187 +0,0 @@
package frc.robot.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.constants.VisionConstants;
public class Vision{
private NetworkTable blackVisionTable;
private DoubleSubscriber black_tx;
private DoubleSubscriber black_ty;
private DoubleSubscriber black_dist;
private DoubleSubscriber blackClosestTag;
private BooleanSubscriber blackTagDetected;
private DoubleSubscriber blackFramerate;
private NetworkTable orangeVisionTable;
private DoubleSubscriber orange_tx;
private DoubleSubscriber orange_ty;
private DoubleSubscriber orange_dist;
private DoubleSubscriber orangeClosestTag;
private BooleanSubscriber orangeTagDetected;
private DoubleSubscriber orangeFramerate;
private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 14.0-1.05, 7.308+2.75};
private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault();
blackVisionTable = inst.getTable("black_Fiducial");
orangeVisionTable = inst.getTable("orange_Fiducial");
black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
}
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
new Rotation2d());
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
}
public Pose2d cameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
// System.out.println(gyroBuffer.getSample(timestamp));
double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
new Rotation2d());
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
.transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
.getTranslation();
Pose2d robotPose = new Pose2d(
fieldToCameraTranslation,
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
return robotPose;
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
}
public double getBlackTX(){
return black_tx.get();
}
public double getBlackTY(){
return black_ty.get();
}
public double getBlackDist(){
return black_dist.get();
}
public int getBlackClosestTag(){
return (int) blackClosestTag.get();
}
public double getBlackTimeStamp(){
return black_tx.getLastChange()-VisionConstants.latencyFudge;
}
public boolean getBlackTagDetected(){
return blackTagDetected.get();
}
public double getBlackFPS(){
return blackFramerate.get();
}
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
);
}else{
return new Pose2d();
}
}
public double getOrangeTX(){
return orange_tx.get();
}
public double getOrangeTY(){
return orange_ty.get();
}
public double getOrangeDist(){
return orange_dist.get();
}
public int getOrangeClosestTag(){
return (int) orangeClosestTag.get();
}
public double getOrangeTimeStamp(){
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
}
public boolean getOrangeTagDetected(){
return orangeTagDetected.get();
}
public double getOrangeFPS(){
return orangeFramerate.get();
}
public boolean isBlackConnected(){
return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
}
public boolean isOrangeConnected(){
return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
}
}

17
src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java
View File

@ -1,8 +1,8 @@
package frc.robot.subsystems.sysid; package frc.robot.subsystems.sysid;
import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.MetersPerSecond;
import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutDistance;
import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutLinearVelocity;
@ -27,9 +27,9 @@ public class ElevatorSysID extends Elevator {
appliedVoltage = Volts.mutable(0); appliedVoltage = Volts.mutable(0);
elevatorPosition = Inches.mutable(0); elevatorPosition = Meters.mutable(0);
elevatorVelocity = InchesPerSecond.mutable(0); elevatorVelocity = MetersPerSecond.mutable(0);
routine = new SysIdRoutine( routine = new SysIdRoutine(
ElevatorConstants.kSysIDConfig, ElevatorConstants.kSysIDConfig,
@ -41,10 +41,10 @@ public class ElevatorSysID extends Elevator {
elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
)) ))
.linearPosition(elevatorPosition.mut_replace( .linearPosition(elevatorPosition.mut_replace(
encoder.getPosition(), Inches encoder.getPosition(), Meters
)) ))
.linearVelocity(elevatorVelocity.mut_replace( .linearVelocity(elevatorVelocity.mut_replace(
encoder.getVelocity(), InchesPerSecond encoder.getVelocity(), MetersPerSecond
)); ));
}, },
this this
@ -52,11 +52,6 @@ public class ElevatorSysID extends Elevator {
); );
} }
@Override
public void periodic() {
}
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return routine.quasistatic(direction); return routine.quasistatic(direction);
} }

62
src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.java
View File

@ -1,62 +0,0 @@
package frc.robot.subsystems.sysid;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import edu.wpi.first.units.measure.MutAngle;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.constants.ManipulatorPivotConstants;
import frc.robot.subsystems.ManipulatorPivot;
public class ManipulatorPivotSysID extends ManipulatorPivot {
private MutVoltage appliedVoltage;
private MutAngle pivotPosition;
private MutAngularVelocity pivotVelocity;
private SysIdRoutine routine;
public ManipulatorPivotSysID() {
super();
appliedVoltage = Volts.mutable(0);
pivotPosition = Radians.mutable(0);
pivotVelocity = RadiansPerSecond.mutable(0);
routine = new SysIdRoutine(
ManipulatorPivotConstants.kSysIDConfig,
new SysIdRoutine.Mechanism(
pivotMotor::setVoltage,
(log) -> {
log.motor("armMotor")
.voltage(appliedVoltage.mut_replace(
pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
))
.angularPosition(pivotPosition.mut_replace(
getEncoderPosition(), Radians
))
.angularVelocity(pivotVelocity.mut_replace(
getEncoderVelocity(), RadiansPerSecond
));
},
this
)
);
}
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return routine.quasistatic(direction);
}
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return routine.dynamic(direction);
}
}

35
vendordeps/AdvantageKit.json
View File

@ -1,35 +0,0 @@
{
"fileName": "AdvantageKit.json",
"name": "AdvantageKit",
"version": "4.1.1",
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
"frcYear": "2025",
"mavenUrls": [
"https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
],
"jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
"javaDependencies": [
{
"groupId": "org.littletonrobotics.akit",
"artifactId": "akit-java",
"version": "4.1.1"
}
],
"jniDependencies": [
{
"groupId": "org.littletonrobotics.akit",
"artifactId": "akit-wpilibio",
"version": "4.1.1",
"skipInvalidPlatforms": false,
"isJar": false,
"validPlatforms": [
"linuxathena",
"linuxx86-64",
"linuxarm64",
"osxuniversal",
"windowsx86-64"
]
}
],
"cppDependencies": []
}