Compare commits
No commits in common. "main" and "kraken_swerve" have entirely different histories.
main
...
kraken_swe
@ -1,6 +0,0 @@
|
||||
{
|
||||
"download": {
|
||||
"localDir": "C:\\Users\\infin\\Downloads",
|
||||
"serverTeam": "2648"
|
||||
}
|
||||
}
|
||||
@ -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 +0,0 @@
|
||||
{}
|
||||
@ -10,11 +10,6 @@ java {
|
||||
|
||||
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)
|
||||
// This is added by GradleRIO's backing project DeployUtils.
|
||||
deploy {
|
||||
@ -77,9 +72,6 @@ dependencies {
|
||||
|
||||
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
|
||||
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 {
|
||||
|
||||
Binary file not shown.
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
}
|
||||
@ -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": []
|
||||
}
|
||||
@ -4,15 +4,7 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import org.littletonrobotics.junction.LoggedRobot;
|
||||
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.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
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
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends LoggedRobot {
|
||||
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
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
|
||||
* initialization code.
|
||||
|
||||
@ -4,26 +4,20 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import frc.robot.constants.ManipulatorPivotConstants;
|
||||
import frc.robot.constants.ClimberPivotConstants;
|
||||
import frc.robot.constants.ArmConstants;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
import frc.robot.subsystems.ManipulatorPivot;
|
||||
import frc.robot.subsystems.Arm;
|
||||
import frc.robot.subsystems.ClimberPivot;
|
||||
import frc.robot.subsystems.ClimberRollers;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
import frc.robot.subsystems.Elevator;
|
||||
import frc.robot.subsystems.Indexer;
|
||||
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.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
@ -33,6 +27,8 @@ import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
|
||||
public class RobotContainer {
|
||||
private Arm arm;
|
||||
|
||||
private ClimberPivot climberPivot;
|
||||
|
||||
private ClimberRollers climberRollers;
|
||||
@ -40,22 +36,19 @@ public class RobotContainer {
|
||||
private Drivetrain drivetrain;
|
||||
|
||||
private Elevator elevator;
|
||||
//private ElevatorSysID elevator;
|
||||
|
||||
//private Indexer indexer;
|
||||
private Indexer indexer;
|
||||
|
||||
private Manipulator manipulator;
|
||||
|
||||
private ManipulatorPivot manipulatorPivot;
|
||||
|
||||
private CommandXboxController driver;
|
||||
private CommandXboxController operator;
|
||||
|
||||
private SendableChooser<Command> autoChooser;
|
||||
|
||||
private IntSupplier closestTag;
|
||||
|
||||
public RobotContainer() {
|
||||
arm = new Arm();
|
||||
|
||||
climberPivot = new ClimberPivot();
|
||||
|
||||
climberRollers = new ClimberRollers();
|
||||
@ -63,323 +56,116 @@ public class RobotContainer {
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
elevator = new Elevator();
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
//indexer = new Indexer();
|
||||
indexer = new Indexer();
|
||||
|
||||
manipulator = new Manipulator();
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
|
||||
configureNamedCommands();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
||||
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();
|
||||
//elevatorSysIDBindings();
|
||||
//elevatorOnlyBindings();
|
||||
|
||||
configureNamedCommands();
|
||||
|
||||
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() {
|
||||
//Default commands
|
||||
arm.setDefaultCommand(
|
||||
arm.goToSetpoint(0, 1)
|
||||
);
|
||||
|
||||
climberPivot.setDefaultCommand(
|
||||
climberPivot.runPivot(() -> 0)
|
||||
climberPivot.goToAngle(0, 1)
|
||||
);
|
||||
|
||||
climberRollers.setDefaultCommand(
|
||||
climberRollers.runRoller(() -> 0)
|
||||
climberRollers.runRoller(0)
|
||||
);
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> driver.getLeftY(),
|
||||
() -> driver.getLeftX(),
|
||||
() -> driver.getRightX(),
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver::getRightX,
|
||||
() -> true
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
elevator.setDefaultCommand(
|
||||
elevator.maintainPosition()
|
||||
elevator.runAssistedElevator(operator::getLeftY)
|
||||
);
|
||||
|
||||
//indexer.setDefaultCommand(
|
||||
// indexer.runIndexer(() -> 0)
|
||||
//);
|
||||
|
||||
|
||||
manipulatorPivot.setDefaultCommand(
|
||||
manipulatorPivot.maintainPosition()
|
||||
indexer.setDefaultCommand(
|
||||
indexer.runIndexer(0)
|
||||
);
|
||||
|
||||
|
||||
manipulator.setDefaultCommand(
|
||||
manipulator.runManipulator(() -> 0.0, false)
|
||||
manipulator.runManipulator(0)
|
||||
);
|
||||
|
||||
//Driver inputs
|
||||
|
||||
driver.start().whileTrue(
|
||||
drivetrain.setXCommand()
|
||||
);
|
||||
|
||||
driver.rightTrigger().whileTrue(
|
||||
manipulator.runManipulator(() -> 0.35, true)
|
||||
);
|
||||
|
||||
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))
|
||||
))
|
||||
manipulator.runManipulator(1)
|
||||
);
|
||||
|
||||
//Operator inputs
|
||||
operator.povUp().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL4Position,
|
||||
ManipulatorPivotConstants.kL4Position
|
||||
moveManipulator(
|
||||
ElevatorConstants.kElevatorL4Position,
|
||||
ArmConstants.kArmL4Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povRight().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL3Position,
|
||||
ManipulatorPivotConstants.kL3Position
|
||||
moveManipulator(
|
||||
ElevatorConstants.kElevatorL3Position,
|
||||
ArmConstants.kArmL3Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povLeft().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL2Position,
|
||||
ManipulatorPivotConstants.kL2Position
|
||||
moveManipulator(
|
||||
ElevatorConstants.kElevatorL2Position,
|
||||
ArmConstants.kArmL2Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povDown().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL1Position,
|
||||
ManipulatorPivotConstants.kL1Position
|
||||
moveManipulator(
|
||||
ElevatorConstants.kElevatorL1Position,
|
||||
ArmConstants.kArmL1Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.back().onTrue(elevator.homeCommand());
|
||||
|
||||
operator.start().toggleOnTrue(
|
||||
climberPivot.runPivot(() -> -operator.getRightY())
|
||||
.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
|
||||
|
||||
operator.a().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
|
||||
coralIntakeRoutine()
|
||||
);
|
||||
|
||||
operator.x().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
algaeIntakeRoutine(true)
|
||||
);
|
||||
|
||||
operator.b().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
|
||||
.alongWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.until(() -> driver.a().getAsBoolean())
|
||||
algaeIntakeRoutine(false)
|
||||
);
|
||||
|
||||
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?
|
||||
);
|
||||
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
||||
}
|
||||
|
||||
//creates tabs and transforms them on the shuffleboard
|
||||
private void configureShuffleboard() {
|
||||
ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
|
||||
ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
|
||||
ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
|
||||
|
||||
Shuffleboard.selectTab(OIConstants.kAutoTab);
|
||||
|
||||
@ -388,257 +174,84 @@ public class RobotContainer {
|
||||
.withPosition(0, 0)
|
||||
.withWidget(BuiltInWidgets.kComboBoxChooser);
|
||||
|
||||
sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
|
||||
sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
|
||||
.withSize(2, 1)
|
||||
.withPosition(0, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
|
||||
sensorTab.addDouble("ArmPosition", arm::getEncoderPosition)
|
||||
.withSize(2, 1)
|
||||
.withPosition(2, 0)
|
||||
.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() {
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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")
|
||||
//teleop routines
|
||||
private Command coralIntakeRoutine() {
|
||||
return moveManipulator(
|
||||
ElevatorConstants.kCoralIntakePosition,
|
||||
ManipulatorPivotConstants.kCoralIntakePosition
|
||||
ElevatorConstants.kElevatorCoralIntakePosition,
|
||||
ArmConstants.kArmCoralIntakePosition
|
||||
)
|
||||
.andThen(manipulator.runUntilCollected(() -> .5));
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
return moveManipulator(
|
||||
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
|
||||
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
|
||||
)
|
||||
.andThen(manipulator.runUntilCollected(() -> 1));
|
||||
.andThen(manipulator.runUntilCollected(1, true));
|
||||
}
|
||||
|
||||
private Command algaeIntakeRoutine(boolean l2) {
|
||||
return moveManipulator(
|
||||
l2 ? ElevatorConstants.kElevatorL2AlgaePosition : ElevatorConstants.kElevatorL3AlgaePosition,
|
||||
l2 ? ArmConstants.kArmL2AlgaePosition : ArmConstants.kArmL3AlgaePosition
|
||||
)
|
||||
.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) {
|
||||
// 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
|
||||
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);
|
||||
// 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
|
||||
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
|
||||
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
|
||||
} else if (!arm.isMotionSafe(armPosition) && !arm.isMotionSafe()) {
|
||||
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
|
||||
.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
|
||||
} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
|
||||
} else if (!arm.isMotionSafe(armPosition) && arm.isMotionSafe()) {
|
||||
return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
|
||||
// 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);
|
||||
// Catch all command that's safe regardless of arm and elevator positions
|
||||
} else {
|
||||
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
|
||||
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
|
||||
.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) {
|
||||
|
||||
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
|
||||
armPosition = MathUtil.clamp(
|
||||
armPosition,
|
||||
0,
|
||||
ManipulatorPivotConstants.kRotationLimit
|
||||
);
|
||||
}*/
|
||||
|
||||
return Commands.either(
|
||||
Commands.either(
|
||||
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
elevator.goToSetpoint(elevatorPosition, 2).andThen(arm.goToSetpoint(armPosition, 2)),
|
||||
elevator.goToSetpoint(elevatorPosition, 2).alongWith(arm.goToSetpoint(armPosition, 2)),
|
||||
() -> sequential
|
||||
),
|
||||
Commands.either(
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
|
||||
arm.goToSetpoint(armPosition, 2).andThen(elevator.goToSetpoint(elevatorPosition, 2)),
|
||||
arm.goToSetpoint(armPosition, 2).alongWith(elevator.goToSetpoint(elevatorPosition, 2)),
|
||||
() -> sequential
|
||||
),
|
||||
() -> 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
|
||||
/*
|
||||
* A moveManipulator method that will guarantee a safe movement.
|
||||
* Here in case we need want to skip moveManipulator debugging
|
||||
*/
|
||||
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));
|
||||
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
|
||||
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
|
||||
.andThen(arm.goToSetpoint(armPosition, 2));
|
||||
}
|
||||
}
|
||||
79
src/main/java/frc/robot/constants/ArmConstants.java
Normal file
79
src/main/java/frc/robot/constants/ArmConstants.java
Normal file
@ -0,0 +1,79 @@
|
||||
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.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.signals.SensorDirectionValue;
|
||||
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 ArmConstants {
|
||||
public static final int kArmMotorID = 0;
|
||||
public static final int kCANcoderID = 0;
|
||||
|
||||
public static final int kMotorAmpsMax = 0;
|
||||
|
||||
public static final double kArmMaxVelocity = 0;
|
||||
|
||||
public static final double kPositionalP = 0;
|
||||
public static final double kPositionalI = 0;
|
||||
public static final double kPositionalD = 0;
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(1);
|
||||
|
||||
public static final double kVelocityP = 0;
|
||||
public static final double kVelocityI = 0;
|
||||
public static final double kVelocityD = 0;
|
||||
// TODO Is this reasonable?
|
||||
public static final double kVelocityTolerance = Units.degreesToRadians(3) / 60;
|
||||
|
||||
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 double kMagnetOffset = 0.0;
|
||||
public static final double kAbsoluteSensorDiscontinuityPoint = 0.0;
|
||||
|
||||
public static final double kSysIDRampRate = 1;
|
||||
public static final double kSysIDStepVolts = 7;
|
||||
public static final double kSysIDTimeout = 10;
|
||||
|
||||
public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
|
||||
|
||||
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 CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
|
||||
canCoderConfig.MagnetSensor.MagnetOffset = 0.0;
|
||||
// TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0
|
||||
//canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
|
||||
|
||||
motorConfig
|
||||
.smartCurrentLimit(kMotorAmpsMax)
|
||||
.idleMode(kIdleMode);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@ -11,32 +11,25 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
public class AutoConstants {
|
||||
public static final double kMaxSpeedMetersPerSecond = 5;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
||||
public static final double kMaxSpeedMetersPerSecond = 3;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
|
||||
|
||||
public static final double kPXYController = 3.5;
|
||||
public static final double kPThetaController = 5;
|
||||
|
||||
public static final double kAlignPXYController = 2;
|
||||
public static final double kAlignPThetaController = 5;
|
||||
public static final double kPXController = 1;
|
||||
public static final double kPYController = 1;
|
||||
public static final double kPThetaController = 1;
|
||||
|
||||
// Constraint for the motion profiled robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
|
||||
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)
|
||||
new PIDConstants(kPXController, 0, 0),
|
||||
new PIDConstants(kPYController, 0, 0)
|
||||
);
|
||||
|
||||
static {
|
||||
|
||||
@ -1,9 +1,7 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class ClimberPivotConstants {
|
||||
public static final int kPivotMotorID = 10;
|
||||
public static final int kPivotMotorID = 0;
|
||||
|
||||
public static final int kClimberLimitSwitchID = 0;
|
||||
|
||||
@ -12,9 +10,4 @@ public class ClimberPivotConstants {
|
||||
public static final double kPIDControllerP = 0;
|
||||
public static final double kPIDControllerI = 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();
|
||||
}
|
||||
|
||||
@ -1,18 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
public class ClimberRollersConstants {
|
||||
public static final int kRollerMotorID = 9;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
motorConfig
|
||||
.smartCurrentLimit(40)
|
||||
.idleMode(IdleMode.kBrake)
|
||||
.inverted(true);
|
||||
|
||||
}
|
||||
public static final int kRollerMotorID = 0;
|
||||
}
|
||||
|
||||
@ -13,26 +13,19 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
public class DrivetrainConstants {
|
||||
// Driving Parameters - Note that these are not the maximum capable speeds of
|
||||
// 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
|
||||
|
||||
// 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
|
||||
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
|
||||
/*
|
||||
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;
|
||||
*/
|
||||
|
||||
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;
|
||||
public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
|
||||
public static final double kFrontRightChassisAngularOffset = 0;
|
||||
public static final double kBackLeftChassisAngularOffset = Math.PI;
|
||||
public static final double kBackRightChassisAngularOffset = Math.PI / 2;
|
||||
|
||||
// 1, 7, 10 is the default for these three values
|
||||
public static final double kSysIDDrivingRampRate = 1;
|
||||
@ -45,23 +38,17 @@ public class DrivetrainConstants {
|
||||
public static final double kSysIDTurningTimeout = 10;
|
||||
|
||||
// SPARK MAX CAN IDs
|
||||
public static final int kFrontLeftDrivingCanId = 0;
|
||||
public static final int kRearLeftDrivingCanId = 2;
|
||||
public static final int kFrontRightDrivingCanId = 1;
|
||||
public static final int kRearRightDrivingCanId = 3;
|
||||
public static final int kFrontLeftDrivingCanId = 11;
|
||||
public static final int kRearLeftDrivingCanId = 13;
|
||||
public static final int kFrontRightDrivingCanId = 15;
|
||||
public static final int kRearRightDrivingCanId = 17;
|
||||
|
||||
public static final int kFrontLeftTurningCanId = 2;
|
||||
public static final int kRearLeftTurningCanId = 4;
|
||||
public static final int kFrontRightTurningCanId = 7;
|
||||
public static final int kRearRightTurningCanId = 5;
|
||||
|
||||
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 int kFrontLeftTurningCanId = 10;
|
||||
public static final int kRearLeftTurningCanId = 12;
|
||||
public static final int kFrontRightTurningCanId = 14;
|
||||
public static final int kRearRightTurningCanId = 16;
|
||||
|
||||
public static final boolean kGyroReversed = false;
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
|
||||
@ -11,56 +11,43 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
|
||||
public class ElevatorConstants {
|
||||
public static final int kElevatorMotor1ID = 8;
|
||||
public static final int kElevatorMotor2ID = 6;
|
||||
public static final int kElevatorMotor1ID = 0;
|
||||
public static final int kElevatorMotor2ID = 0;
|
||||
|
||||
public static final int kTopLimitSwitchID = 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 kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
|
||||
public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
|
||||
public static final double kEncoderConversionFactor = 0;
|
||||
|
||||
public static final int kCurrentLimit = 40;
|
||||
public static final int kMotorAmpsMax = 0;
|
||||
|
||||
public static final double kUpControllerP = 5.6;//7; //
|
||||
public static final double kUpControllerI = 0;
|
||||
public static final double kUpControllerD = 0.28;//0.28
|
||||
public static final double kPositionControllerP = 0;
|
||||
public static final double kPositionControllerI = 0;
|
||||
public static final double kPositionControllerD = 0;
|
||||
|
||||
public static final double kVelocityControllerP = 0;
|
||||
public static final double kVelocityControllerI = 0;
|
||||
public static final double kVelocityControllerD = 0;
|
||||
|
||||
public static final double kDownControllerP = 5.6;//7; //
|
||||
public static final double kDownControllerI = 0;
|
||||
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
|
||||
public static final double kFeedForwardS = 0;
|
||||
public static final double kFeedForwardG = 0;
|
||||
public static final double kFeedForwardV = 0;
|
||||
|
||||
public static final double kMaintainP = 3;
|
||||
|
||||
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 kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
|
||||
public static final double kFeedForwardV = 0.12; // calculated value 0.12
|
||||
|
||||
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
|
||||
public static final double kCoralIntakePosition = 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;
|
||||
public static final double kElevatorCoralIntakePosition = 0;
|
||||
public static final double kElevatorL1Position = 0;
|
||||
public static final double kElevatorL2Position = 0;
|
||||
public static final double kElevatorL3Position = 0;
|
||||
public static final double kElevatorL4Position = 0;
|
||||
public static final double kElevatorL2AlgaePosition = 0;
|
||||
public static final double kElevatorL3AlgaePosition = 0;
|
||||
public static final double kElevatorBracePosition = 0;
|
||||
public static final double kElevatorMaxHeight = 0;
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = .25;
|
||||
public static final double kSysIDStepVolts = 3;
|
||||
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;
|
||||
@ -77,11 +64,10 @@ public class ElevatorConstants {
|
||||
|
||||
static {
|
||||
motorConfig
|
||||
.smartCurrentLimit(kCurrentLimit)
|
||||
.idleMode(kIdleMode)
|
||||
.inverted(true);
|
||||
.smartCurrentLimit(kMotorAmpsMax)
|
||||
.idleMode(kIdleMode);
|
||||
motorConfig.encoder
|
||||
.positionConversionFactor(kEncoderPositionConversionFactor)
|
||||
.velocityConversionFactor(kEncoderVelocityConversionFactor);
|
||||
.positionConversionFactor(kEncoderConversionFactor)
|
||||
.velocityConversionFactor(kEncoderConversionFactor / 60.0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,15 +1,6 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class IndexerConstants {
|
||||
public static final int kIndexerMotorID = 16;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static{
|
||||
motorConfig.smartCurrentLimit(30)
|
||||
.idleMode(IdleMode.kBrake);
|
||||
};
|
||||
public static final int kIndexerMotorID = 0;
|
||||
public static final int kIndexerBeamBreakID = 0;
|
||||
}
|
||||
|
||||
@ -1,16 +1,7 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class ManipulatorConstants {
|
||||
public static final int kManipulatorMotorID = 12;
|
||||
public static final int kCoralBeamBreakID = 2;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static{
|
||||
motorConfig.smartCurrentLimit(40)
|
||||
.idleMode(IdleMode.kBrake);
|
||||
};
|
||||
public static final int kManipulatorMotorID = 0;
|
||||
public static final int kCoralBeamBreakID = 0;
|
||||
public static final int kAlgaeBeamBreakID = 0;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@ -2,7 +2,6 @@ package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
|
||||
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;
|
||||
@ -23,7 +22,7 @@ public class ModuleConstants {
|
||||
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
|
||||
// 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)
|
||||
/ kDrivingMotorReduction;
|
||||
|
||||
@ -42,8 +41,7 @@ public class ModuleConstants {
|
||||
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 kDriveMotorStatorCurrentLimit = 120;
|
||||
public static final int kTurnMotorCurrentLimit = 20;
|
||||
|
||||
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
|
||||
@ -58,22 +56,17 @@ public class ModuleConstants {
|
||||
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 {
|
||||
kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
|
||||
|
||||
kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
|
||||
kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true;
|
||||
kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
|
||||
kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit;
|
||||
|
||||
kDriveMotorConfig.Inverted = kDriveInversionState;
|
||||
kDriveMotorConfig.NeutralMode = kDriveIdleMode;
|
||||
|
||||
kAudioConfig.AllowMusicDurDisable = true;
|
||||
|
||||
kDriveSlot0Config.kP = kDriveP;
|
||||
kDriveSlot0Config.kI = kDriveI;
|
||||
kDriveSlot0Config.kD = kDriveD;
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class NeoMotorConstants {
|
||||
public static final double kFreeSpeedRpm = 6000; //for kraken not neo
|
||||
public static final double kFreeSpeedRpm = 5676;
|
||||
}
|
||||
|
||||
@ -4,11 +4,8 @@ public class OIConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
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 = "Sensors Tab";
|
||||
public static final String kApriltagTab = "Apriltag Tab";
|
||||
|
||||
public static final double kJoystickExponential = 3;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
}
|
||||
117
src/main/java/frc/robot/subsystems/Arm.java
Normal file
117
src/main/java/frc/robot/subsystems/Arm.java
Normal file
@ -0,0 +1,117 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
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.ArmConstants;
|
||||
|
||||
public class Arm extends SubsystemBase {
|
||||
protected SparkMax armMotor;
|
||||
|
||||
private CANcoder canCoder;
|
||||
|
||||
private PIDController positionController;
|
||||
private PIDController velocityController;
|
||||
|
||||
private ArmFeedforward feedForward;
|
||||
|
||||
public Arm() {
|
||||
armMotor = new SparkMax(
|
||||
ArmConstants.kArmMotorID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
|
||||
armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
|
||||
|
||||
positionController = new PIDController(
|
||||
ArmConstants.kPositionalP,
|
||||
ArmConstants.kPositionalI,
|
||||
ArmConstants.kPositionalD
|
||||
);
|
||||
|
||||
// TODO: Generate constants for continuous input range based on CANcoder configuration?
|
||||
positionController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(179));
|
||||
positionController.setTolerance(ArmConstants.kPositionalTolerance);
|
||||
|
||||
velocityController = new PIDController(
|
||||
ArmConstants.kVelocityP,
|
||||
ArmConstants.kVelocityI,
|
||||
ArmConstants.kVelocityD
|
||||
);
|
||||
|
||||
velocityController.setTolerance(ArmConstants.kVelocityTolerance);
|
||||
|
||||
canCoder = new CANcoder(ArmConstants.kCANcoderID);
|
||||
canCoder.getConfigurator().apply(ArmConstants.canCoderConfig);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.getAbsolutePosition().getValueAsDouble());
|
||||
}
|
||||
|
||||
public double getEncoderVelocity() {
|
||||
return Units.rotationsToRadians(canCoder.getVelocity().getValueAsDouble());
|
||||
}
|
||||
}
|
||||
@ -1,13 +1,11 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
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.SubsystemBase;
|
||||
import frc.robot.constants.ClimberPivotConstants;
|
||||
@ -17,41 +15,45 @@ public class ClimberPivot extends SubsystemBase {
|
||||
|
||||
private RelativeEncoder neoEncoder;
|
||||
|
||||
private DigitalInput cageLimitSwitch;
|
||||
|
||||
private PIDController pidController;
|
||||
|
||||
public ClimberPivot() {
|
||||
pivotMotor = new SparkMax(
|
||||
ClimberPivotConstants.kPivotMotorID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
|
||||
pivotMotor.configure(
|
||||
ClimberPivotConstants.motorConfig,
|
||||
ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
|
||||
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(() -> {
|
||||
pivotMotor.set(speed.getAsDouble());
|
||||
pivotMotor.set(speed);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
public Command goToAngle(double setpoint, double timeout) {
|
||||
return run(() -> {
|
||||
pivotMotor.set(speed);
|
||||
}).until(() -> neoEncoder.getPosition() >= setpoint);
|
||||
pivotMotor.set(
|
||||
pidController.calculate(
|
||||
neoEncoder.getPosition(),
|
||||
setpoint
|
||||
)
|
||||
);
|
||||
}).withTimeout(timeout);
|
||||
}
|
||||
|
||||
public double getEncoderPosition() {
|
||||
return neoEncoder.getPosition();
|
||||
public boolean getCageLimitSwitch() {
|
||||
return cageLimitSwitch.get();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,17 +1,12 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
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 {
|
||||
private SparkMax rollerMotor;
|
||||
|
||||
@ -20,23 +15,11 @@ public class ClimberRollers extends SubsystemBase {
|
||||
ClimberRollersConstants.kRollerMotorID,
|
||||
MotorType.kBrushless
|
||||
);
|
||||
|
||||
rollerMotor.configure(
|
||||
ClimberRollersConstants.motorConfig,
|
||||
ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
public Command runRoller(double speed) {
|
||||
return run(() -> {
|
||||
rollerMotor.set(speed.getAsDouble());
|
||||
rollerMotor.set(speed);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
@ -4,45 +4,28 @@
|
||||
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.Optional;
|
||||
import java.util.function.BooleanSupplier;
|
||||
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.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.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.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
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.SubsystemBase;
|
||||
import frc.robot.constants.AutoConstants;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
// Create MAXSwerveModules
|
||||
@ -52,26 +35,10 @@ public class Drivetrain extends SubsystemBase {
|
||||
protected MAXSwerveModule m_rearRight;
|
||||
|
||||
// The gyro sensor
|
||||
private AHRS gyro;
|
||||
private AHRS ahrs;
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
private SwerveDrivePoseEstimator m_estimator;
|
||||
|
||||
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;
|
||||
private SwerveDriveOdometry m_odometry;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain() {
|
||||
@ -99,32 +66,17 @@ public class Drivetrain extends SubsystemBase {
|
||||
DrivetrainConstants.kBackRightChassisAngularOffset
|
||||
);
|
||||
|
||||
gyro = new AHRS(NavXComType.kMXP_SPI);
|
||||
ahrs = new AHRS(NavXComType.kMXP_SPI);
|
||||
|
||||
m_estimator = new SwerveDrivePoseEstimator(
|
||||
m_odometry = new SwerveDriveOdometry(
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
Rotation2d.fromDegrees(ahrs.getAngle()),
|
||||
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,
|
||||
@ -142,31 +94,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
},
|
||||
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(
|
||||
m_odometry.update(
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
@ -174,77 +107,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearLeft.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
|
||||
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
|
||||
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
|
||||
|
||||
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
|
||||
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
|
||||
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
|
||||
}
|
||||
}
|
||||
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() {
|
||||
@ -270,7 +132,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_estimator.getEstimatedPosition();
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -279,8 +141,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
|
||||
m_estimator.resetPosition(
|
||||
m_odometry.resetPosition(
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
@ -295,20 +156,12 @@ public class Drivetrain extends SubsystemBase {
|
||||
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
|
||||
BooleanSupplier fieldRelative) {
|
||||
return run(() -> {
|
||||
if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
|
||||
drive(
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
fieldRelative.getAsBoolean()
|
||||
);
|
||||
}else{
|
||||
drive(
|
||||
MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
fieldRelative.getAsBoolean()
|
||||
);}
|
||||
drive(
|
||||
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
|
||||
fieldRelative.getAsBoolean()
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
@ -323,24 +176,14 @@ public class Drivetrain extends SubsystemBase {
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
// 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 = 0;
|
||||
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 xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
|
||||
double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
|
||||
double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
|
||||
|
||||
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
|
||||
new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
|
||||
Rotation2d.fromDegrees(getGyroValue()))
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||
@ -356,71 +199,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.
|
||||
*/
|
||||
@ -453,17 +231,13 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearRight.resetEncoders();
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot.
|
||||
* @return */
|
||||
public Command zeroHeading() {
|
||||
return run(() -> {
|
||||
gyro.reset();
|
||||
m_estimator.resetRotation(new Rotation2d(0));
|
||||
});
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
ahrs.reset();;
|
||||
}
|
||||
|
||||
public double getGyroValue() {
|
||||
return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
|
||||
return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -472,11 +246,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return m_estimator.getEstimatedPosition().getRotation().getDegrees();
|
||||
}
|
||||
|
||||
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
|
||||
return gyroBuffer;
|
||||
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -485,20 +255,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
return gyro.getRate() * (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()));
|
||||
}
|
||||
});
|
||||
return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -2,15 +2,12 @@ package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
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.PIDController;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
@ -26,10 +23,8 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
|
||||
private PIDController maintainPID;
|
||||
private PIDController positionController;
|
||||
private PIDController velocityController;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
|
||||
@ -62,27 +57,17 @@ public class Elevator extends SubsystemBase {
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidControllerDown = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
positionController = new PIDController(
|
||||
ElevatorConstants.kPositionControllerP,
|
||||
ElevatorConstants.kPositionControllerI,
|
||||
ElevatorConstants.kPositionControllerD
|
||||
);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
velocityController = new PIDController(
|
||||
ElevatorConstants.kVelocityControllerP,
|
||||
ElevatorConstants.kVelocityControllerI,
|
||||
ElevatorConstants.kVelocityControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
|
||||
|
||||
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
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
|
||||
* and the elevator brace position
|
||||
@ -120,196 +94,60 @@ public class Elevator extends SubsystemBase {
|
||||
* @return Is the motion safe
|
||||
*/
|
||||
public boolean isMotionSafe(double motionTarget) {
|
||||
return motionTarget > ElevatorConstants.kBracePosition;
|
||||
return motionTarget > ElevatorConstants.kElevatorBracePosition;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* A manual translation command that will move the elevator using a consistent velocity disregarding direction
|
||||
*
|
||||
* @param speed How fast the elevator moves
|
||||
* @return Sets motor voltage to move the elevator relative to the speed parameter
|
||||
*/
|
||||
public Command runAssistedElevator(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
|
||||
|
||||
double voltsOut = velocityController.calculate(
|
||||
encoder.getVelocity(),
|
||||
realSpeedTarget
|
||||
) + feedForward.calculate(realSpeedTarget);
|
||||
|
||||
elevatorMotor1.setVoltage(voltsOut);
|
||||
}).until(
|
||||
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
|
||||
}
|
||||
|
||||
/**
|
||||
* A manual translation command that uses feed forward calculation to maintain position
|
||||
*
|
||||
* @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) {
|
||||
public Command runManualElevator(double speed) {
|
||||
return run(() -> {
|
||||
double desired = speed.getAsDouble();
|
||||
|
||||
if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
|
||||
elevatorMotor1.set(
|
||||
speed.getAsDouble()
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(feedForward.calculate(0));
|
||||
}
|
||||
|
||||
elevatorMotor1.set(speed);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* A command that will use the feed forward to hold up the elevator.
|
||||
* 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(() -> {
|
||||
elevatorMotor1.setVoltage(0.5);
|
||||
})
|
||||
.until(() -> elevatorMotor1.getOutputCurrent() > 5)
|
||||
.andThen(run(() -> encoder.setPosition(0)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
* 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));
|
||||
public Command goToSetpoint(double setpoint, double timeout) {
|
||||
return run(() -> {
|
||||
double voltsOut = positionController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint
|
||||
) + feedForward.calculate(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());
|
||||
}
|
||||
}
|
||||
elevatorMotor1.setVoltage(voltsOut);
|
||||
}).until(
|
||||
() -> positionController.atSetpoint() || bottomLimitSwitch.get()
|
||||
).withTimeout(timeout);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current encoder position
|
||||
@ -320,7 +158,6 @@ public class Elevator extends SubsystemBase {
|
||||
return encoder.getPosition();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
|
||||
*
|
||||
@ -329,38 +166,4 @@ public class Elevator extends SubsystemBase {
|
||||
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
36
src/main/java/frc/robot/subsystems/Indexer.java
Normal 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);
|
||||
}
|
||||
}
|
||||
@ -22,126 +22,120 @@ import com.revrobotics.AbsoluteEncoder;
|
||||
import frc.robot.constants.ModuleConstants;
|
||||
|
||||
public class MAXSwerveModule {
|
||||
private final TalonFX m_drive;
|
||||
private final SparkMax m_turningSpark;
|
||||
private final TalonFX m_drive;
|
||||
private final SparkMax m_turningSpark;
|
||||
|
||||
private final AbsoluteEncoder m_turningEncoder;
|
||||
private final AbsoluteEncoder m_turningEncoder;
|
||||
|
||||
private final SparkClosedLoopController m_turningClosedLoopController;
|
||||
private final SparkClosedLoopController m_turningClosedLoopController;
|
||||
|
||||
private final VelocityVoltage driveVelocityRequest;
|
||||
private final VelocityVoltage driveVelocityRequest;
|
||||
|
||||
private double m_chassisAngularOffset = 0;
|
||||
private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
|
||||
private double m_chassisAngularOffset = 0;
|
||||
private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
|
||||
|
||||
/**
|
||||
* Constructs a MAXSwerveModule and configures the driving and turning motor,
|
||||
* encoder, and PID controller. This configuration is specific to the REV
|
||||
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
|
||||
* Encoder.
|
||||
*/
|
||||
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
|
||||
m_drive = new TalonFX(drivingCANId);
|
||||
m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
|
||||
/**
|
||||
* Constructs a MAXSwerveModule and configures the driving and turning motor,
|
||||
* encoder, and PID controller. This configuration is specific to the REV
|
||||
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
|
||||
* Encoder.
|
||||
*/
|
||||
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
|
||||
m_drive = new TalonFX(drivingCANId);
|
||||
m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
|
||||
|
||||
m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
|
||||
m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
|
||||
|
||||
m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
|
||||
m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
|
||||
|
||||
driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
|
||||
driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
|
||||
|
||||
// Apply the respective configurations to the SPARKS. Reset parameters before
|
||||
// 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.
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
|
||||
// Apply the respective configurations to the SPARKS. Reset parameters before
|
||||
// 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.
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
|
||||
m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
|
||||
|
||||
m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters);
|
||||
m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters);
|
||||
|
||||
m_chassisAngularOffset = chassisAngularOffset;
|
||||
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
|
||||
m_drive.setPosition(0);
|
||||
}
|
||||
m_chassisAngularOffset = chassisAngularOffset;
|
||||
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
|
||||
m_drive.setPosition(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current position of the module.
|
||||
*
|
||||
* @return The current position of the module.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
/**
|
||||
* Returns the current position of the module.
|
||||
*
|
||||
* @return The current position of the module.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
// Apply chassis angular offset to the encoder position to get the position
|
||||
// relative to the chassis.
|
||||
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the desired state for the module.
|
||||
*
|
||||
* @param desiredState Desired state with speed and angle.
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
// Apply chassis angular offset to the desired state.
|
||||
SwerveModuleState correctedDesiredState = new SwerveModuleState();
|
||||
correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
|
||||
correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
|
||||
/**
|
||||
* Sets the desired state for the module.
|
||||
*
|
||||
* @param desiredState Desired state with speed and angle.
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
// Apply chassis angular offset to the desired state.
|
||||
SwerveModuleState correctedDesiredState = new SwerveModuleState();
|
||||
correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond;
|
||||
correctedDesiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(m_chassisAngularOffset));
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees.
|
||||
correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees.
|
||||
correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
|
||||
|
||||
// Command driving and turning SPARKS towards their respective setpoints.
|
||||
m_drive.setControl(
|
||||
driveVelocityRequest.withVelocity(
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
).withFeedForward(
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
)
|
||||
);
|
||||
// Command driving and turning SPARKS towards their respective setpoints.
|
||||
m_drive.setControl(
|
||||
driveVelocityRequest.withVelocity(
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
).withFeedForward(
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
)
|
||||
);
|
||||
|
||||
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){
|
||||
m_drive.setVoltage(voltage);
|
||||
}
|
||||
public void setVoltageDrive(double voltage){
|
||||
m_drive.setVoltage(voltage);
|
||||
}
|
||||
|
||||
public void setVoltageTurn(double voltage) {
|
||||
m_turningSpark.setVoltage(voltage);
|
||||
}
|
||||
public void setVoltageTurn(double voltage) {
|
||||
m_turningSpark.setVoltage(voltage);
|
||||
}
|
||||
|
||||
public double getVoltageDrive() {
|
||||
return m_drive.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
public double getVoltageDrive() {
|
||||
return m_drive.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
|
||||
public double getVoltageTurn() {
|
||||
return m_turningSpark.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
|
||||
public TalonFX getDrivingMotor(){
|
||||
return m_drive;
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_drive.setPosition(0);
|
||||
}
|
||||
public double getVoltageTurn() {
|
||||
return m_turningSpark.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_drive.setPosition(0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,26 +1,18 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
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;
|
||||
import frc.robot.constants.ManipulatorConstants;
|
||||
|
||||
public class Manipulator extends SubsystemBase {
|
||||
private SparkMax manipulatorMotor;
|
||||
|
||||
private SparkMax indexerMotor;
|
||||
|
||||
private DigitalInput coralBeamBreak;
|
||||
private DigitalInput algaeBeamBreak;
|
||||
|
||||
public Manipulator() {
|
||||
manipulatorMotor = new SparkMax(
|
||||
@ -28,116 +20,19 @@ public class Manipulator extends SubsystemBase {
|
||||
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);
|
||||
algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
|
||||
}
|
||||
|
||||
@Override
|
||||
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() {
|
||||
public Command runManipulator(double speed) {
|
||||
return run(() -> {
|
||||
runUntilCollected(() -> 0.1);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(() -> {
|
||||
manipulatorMotor.set(
|
||||
coral ? speed.getAsDouble() : speed.getAsDouble() * -1
|
||||
);
|
||||
|
||||
indexerMotor.set(0);
|
||||
manipulatorMotor.set(speed);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
public Command runUntilCollected(double speed, boolean coral) {
|
||||
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();
|
||||
manipulatorMotor.set(coral ? speed : speed * -1);
|
||||
}).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
|
||||
}
|
||||
}
|
||||
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@ -10,40 +10,40 @@ 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;
|
||||
import frc.robot.constants.ArmConstants;
|
||||
import frc.robot.subsystems.Arm;
|
||||
|
||||
public class ManipulatorPivotSysID extends ManipulatorPivot {
|
||||
public class ArmSysID extends Arm {
|
||||
private MutVoltage appliedVoltage;
|
||||
|
||||
private MutAngle pivotPosition;
|
||||
private MutAngle armPosition;
|
||||
|
||||
private MutAngularVelocity pivotVelocity;
|
||||
private MutAngularVelocity armVelocity;
|
||||
|
||||
private SysIdRoutine routine;
|
||||
|
||||
public ManipulatorPivotSysID() {
|
||||
public ArmSysID() {
|
||||
super();
|
||||
|
||||
appliedVoltage = Volts.mutable(0);
|
||||
|
||||
pivotPosition = Radians.mutable(0);
|
||||
armPosition = Radians.mutable(0);
|
||||
|
||||
pivotVelocity = RadiansPerSecond.mutable(0);
|
||||
armVelocity = RadiansPerSecond.mutable(0);
|
||||
|
||||
routine = new SysIdRoutine(
|
||||
ManipulatorPivotConstants.kSysIDConfig,
|
||||
ArmConstants.kSysIDConfig,
|
||||
new SysIdRoutine.Mechanism(
|
||||
pivotMotor::setVoltage,
|
||||
armMotor::setVoltage,
|
||||
(log) -> {
|
||||
log.motor("armMotor")
|
||||
.voltage(appliedVoltage.mut_replace(
|
||||
pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
|
||||
armMotor.get() * RobotController.getBatteryVoltage(), Volts
|
||||
))
|
||||
.angularPosition(pivotPosition.mut_replace(
|
||||
.angularPosition(armPosition.mut_replace(
|
||||
getEncoderPosition(), Radians
|
||||
))
|
||||
.angularVelocity(pivotVelocity.mut_replace(
|
||||
.angularVelocity(armVelocity.mut_replace(
|
||||
getEncoderVelocity(), RadiansPerSecond
|
||||
));
|
||||
},
|
||||
@ -1,8 +1,8 @@
|
||||
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.InchesPerSecond;
|
||||
import static edu.wpi.first.units.Units.MetersPerSecond;
|
||||
|
||||
import edu.wpi.first.units.measure.MutDistance;
|
||||
import edu.wpi.first.units.measure.MutLinearVelocity;
|
||||
@ -27,9 +27,9 @@ public class ElevatorSysID extends Elevator {
|
||||
|
||||
appliedVoltage = Volts.mutable(0);
|
||||
|
||||
elevatorPosition = Inches.mutable(0);
|
||||
elevatorPosition = Meters.mutable(0);
|
||||
|
||||
elevatorVelocity = InchesPerSecond.mutable(0);
|
||||
elevatorVelocity = MetersPerSecond.mutable(0);
|
||||
|
||||
routine = new SysIdRoutine(
|
||||
ElevatorConstants.kSysIDConfig,
|
||||
@ -41,10 +41,10 @@ public class ElevatorSysID extends Elevator {
|
||||
elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
|
||||
))
|
||||
.linearPosition(elevatorPosition.mut_replace(
|
||||
encoder.getPosition(), Inches
|
||||
encoder.getPosition(), Meters
|
||||
))
|
||||
.linearVelocity(elevatorVelocity.mut_replace(
|
||||
encoder.getVelocity(), InchesPerSecond
|
||||
encoder.getVelocity(), MetersPerSecond
|
||||
));
|
||||
},
|
||||
this
|
||||
@ -52,11 +52,6 @@ public class ElevatorSysID extends Elevator {
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
}
|
||||
|
||||
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
|
||||
return routine.quasistatic(direction);
|
||||
}
|
||||
|
||||
@ -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": []
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user