Compare commits
2 Commits
a089dddae3
...
brad_eleva
| Author | SHA1 | Date | |
|---|---|---|---|
| 431c1c2f01 | |||
| 771bcc89e1 |
@@ -1,49 +1,13 @@
|
||||
{
|
||||
"Clients": {
|
||||
"open": true
|
||||
},
|
||||
"Connections": {
|
||||
"open": true
|
||||
},
|
||||
"NetworkTables Settings": {
|
||||
"mode": "Client (NT4)"
|
||||
},
|
||||
"client@2": {
|
||||
"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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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,31 +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"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -10,35 +10,11 @@
|
||||
"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"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -13,38 +13,13 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "J Approach"
|
||||
}
|
||||
},
|
||||
{
|
||||
"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"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "30 Right to HP"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -59,18 +34,6 @@
|
||||
"pathName": "HP to 330 Left"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "K Approach"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "fein"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@@ -34,9 +34,9 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -16,12 +16,12 @@
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.174795081967213,
|
||||
"y": 7.321618852459016
|
||||
"x": 1.3072345890410957,
|
||||
"y": 7.135316780821918
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.031259123063103,
|
||||
"y": 6.375000701774084
|
||||
"x": 2.1636986301369863,
|
||||
"y": 6.188698630136986
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -31,12 +31,19 @@
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "HP Pickup",
|
||||
"waypointRelativePos": 0.20476190476190542,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -46,9 +46,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.053790983606557,
|
||||
"y": 4.312704918032787
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "H"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.389446721311475,
|
||||
"y": 4.3
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.1394487099079695,
|
||||
"y": 4.300997143065429
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -3,40 +3,47 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.174795081967213,
|
||||
"y": 7.321618852459016
|
||||
"x": 1.3072345890410957,
|
||||
"y": 7.135316780821918
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.4670039860768025,
|
||||
"y": 7.096233578486413
|
||||
"x": 2.5994434931506847,
|
||||
"y": 6.909931506849315
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.859118852459017
|
||||
"x": 3.973766447368421,
|
||||
"y": 5.246957236842105
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.304747501684258,
|
||||
"y": 6.385017825061756
|
||||
"x": 3.6582270638067773,
|
||||
"y": 5.772856209444845
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Before K"
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.5,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": 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.115881147540984
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.058811475409836,
|
||||
"y": 5.403586065573771
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "J"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"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.115881147540984
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.11855393929902,
|
||||
"y": 5.383513333352196
|
||||
},
|
||||
"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": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"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.6202868852459016,
|
||||
"y": 5.859118852459017
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.7970635805425386,
|
||||
"y": 5.68234215716238
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Before K"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.8720286885245896,
|
||||
"y": 5.043954918032787
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.836065573770491,
|
||||
"y": 5.295696721311475
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.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": true
|
||||
}
|
||||
@@ -1,65 +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": 12.272,
|
||||
"y": 2.975
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 10.940715672291898,
|
||||
"y": 2.975
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [
|
||||
{
|
||||
"fieldPosition": {
|
||||
"x": 0.4,
|
||||
"y": 5.5
|
||||
},
|
||||
"rotationOffset": 0.0,
|
||||
"minWaypointRelativePos": 0.15,
|
||||
"maxWaypointRelativePos": 0.4,
|
||||
"name": "Point Towards Zone"
|
||||
}
|
||||
],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 59.99999999999999
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -41,9 +41,9 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
|
||||
@@ -3,46 +3,53 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.588217213114754,
|
||||
"y": 7.537397540983607
|
||||
"x": 7.587970890410959,
|
||||
"y": 6.143621575342466
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.3534836065573765,
|
||||
"y": 6.51844262295082
|
||||
"x": 6.385916095890411,
|
||||
"y": 6.158647260273973
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.286577868852459,
|
||||
"y": 5.955020491803278
|
||||
"x": 4.988527397260274,
|
||||
"y": 5.227054794520548
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.872579581181226,
|
||||
"y": 6.826510217830675
|
||||
"x": 5.574529109589041,
|
||||
"y": 6.098544520547945
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Before J"
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.4547619047619047,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -119.71497744813712
|
||||
"rotation": -121.60750224624898
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
|
||||
@@ -1,61 +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.7,
|
||||
"y": 4.3
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.347336065573771,
|
||||
"y": 4.2640368852459005
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "H"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.08214624881291864,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.600204918039607,
|
||||
"y": 6.374590163938041
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.600204918032786,
|
||||
"y": 7.573360655737705
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.209631147540984,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.120696721311476,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 4.745363537952068,
|
||||
"y": 6.074897540983606
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 4.8670081967213115,
|
||||
"y": 6.973975409836065
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.826024590163935,
|
||||
"y": 6.9979508196721305
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 4.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -5,9 +5,9 @@
|
||||
"pathFolders": [],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 4.0,
|
||||
"defaultMaxAccel": 2.5,
|
||||
"defaultMaxAccel": 4.0,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 400.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 48.35,
|
||||
"robotMOI": 6.883,
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -7,9 +7,8 @@ package frc.robot;
|
||||
import frc.robot.constants.ManipulatorPivotConstants;
|
||||
import frc.robot.constants.ClimberPivotConstants;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
import frc.robot.constants.ManipulatorConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.constants.VisionConstants;
|
||||
import frc.robot.constants.ElevatorConstants.ElevatorPositions;
|
||||
import frc.robot.subsystems.ManipulatorPivot;
|
||||
import frc.robot.subsystems.Vision;
|
||||
import frc.robot.subsystems.ClimberPivot;
|
||||
@@ -17,14 +16,10 @@ import frc.robot.subsystems.ClimberRollers;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
import frc.robot.subsystems.Elevator;
|
||||
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.events.EventTrigger;
|
||||
import com.pathplanner.lib.path.EventMarker;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
@@ -32,7 +27,9 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
|
||||
public class RobotContainer {
|
||||
private ClimberPivot climberPivot;
|
||||
@@ -52,35 +49,32 @@ public class RobotContainer {
|
||||
private CommandXboxController operator;
|
||||
|
||||
private SendableChooser<Command> autoChooser;
|
||||
|
||||
private IntSupplier closestTag;
|
||||
private Vision vision;
|
||||
|
||||
public RobotContainer() {
|
||||
climberPivot = new ClimberPivot();
|
||||
|
||||
climberRollers = new ClimberRollers();
|
||||
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
elevator = new Elevator();
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
manipulator = new Manipulator();
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
//vision = new Vision(drivetrain::getGyroValue);
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
configureNamedCommands();
|
||||
elevator = new Elevator(manipulator::getAlgaePhotoswitch);
|
||||
//elevator = new ElevatorSysID();
|
||||
|
||||
manipulatorPivot = new ManipulatorPivot();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
||||
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
||||
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
|
||||
closestTag = drivetrain::getClosestTag;
|
||||
|
||||
configureButtonBindings();
|
||||
//elevatorSysIDBindings();
|
||||
//elevatorOnlyBindings();
|
||||
|
||||
configureNamedCommands();
|
||||
|
||||
configureShuffleboard();
|
||||
}
|
||||
@@ -97,27 +91,22 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
//Default commands
|
||||
climberPivot.setDefaultCommand(
|
||||
climberPivot.runPivot(() -> 0)
|
||||
climberPivot.runPivot(0)
|
||||
);
|
||||
|
||||
climberRollers.setDefaultCommand(
|
||||
climberRollers.runRoller(() -> 0)
|
||||
climberRollers.runRoller(0)
|
||||
);
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
driver::getRightX,
|
||||
() -> true
|
||||
)
|
||||
);
|
||||
|
||||
elevator.setDefaultCommand(
|
||||
elevator.maintainPosition()
|
||||
);
|
||||
|
||||
|
||||
manipulatorPivot.setDefaultCommand(
|
||||
manipulatorPivot.maintainPosition()
|
||||
);
|
||||
@@ -125,7 +114,7 @@ public class RobotContainer {
|
||||
|
||||
manipulator.setDefaultCommand(
|
||||
manipulator.runUntilCollected(
|
||||
() -> 0.0
|
||||
() -> 0
|
||||
)
|
||||
);
|
||||
|
||||
@@ -136,7 +125,7 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
driver.rightTrigger().whileTrue(
|
||||
manipulator.runManipulator(() -> 0.35, true)
|
||||
manipulator.runManipulator(() -> 1, true)
|
||||
);
|
||||
|
||||
driver.leftTrigger().whileTrue(
|
||||
@@ -146,102 +135,73 @@ public class RobotContainer {
|
||||
driver.start().and(driver.back()).onTrue(
|
||||
startingConfig()
|
||||
);
|
||||
|
||||
driver.y().whileTrue(drivetrain.zeroHeading());
|
||||
|
||||
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
|
||||
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
|
||||
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
|
||||
|
||||
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
|
||||
driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
|
||||
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
|
||||
|
||||
driver.start().whileTrue(drivetrain.resetToVision());
|
||||
|
||||
driver.rightBumper().whileTrue(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
|
||||
() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
|
||||
)
|
||||
);
|
||||
|
||||
driver.leftBumper().whileTrue(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
|
||||
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
|
||||
() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
|
||||
)
|
||||
);
|
||||
|
||||
//Operator inputs
|
||||
operator.povUp().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL4Position,
|
||||
ElevatorPositions.kL4,
|
||||
ManipulatorPivotConstants.kL4Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povRight().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL3Position,
|
||||
ElevatorPositions.kL3,
|
||||
ManipulatorPivotConstants.kL3Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povLeft().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL2Position,
|
||||
ElevatorPositions.kL2,
|
||||
ManipulatorPivotConstants.kL2Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.povDown().onTrue(
|
||||
safeMoveManipulator(
|
||||
ElevatorConstants.kL1Position,
|
||||
ElevatorPositions.kL1,
|
||||
ManipulatorPivotConstants.kL1Position
|
||||
)
|
||||
);
|
||||
|
||||
operator.back().onTrue(elevator.homeCommand());
|
||||
|
||||
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
||||
|
||||
operator.a().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kL1Position, 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.y().onTrue(
|
||||
elevator.setTargetPosition(ElevatorPositions.kL2)
|
||||
);
|
||||
|
||||
new Trigger(() -> Math.abs(MathUtil.applyDeadband(operator.getLeftY(), .05)) > .05).onTrue(
|
||||
elevator.runManualElevator(operator::getLeftY)
|
||||
);
|
||||
|
||||
|
||||
}
|
||||
private void configureNamedCommands() {
|
||||
new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
|
||||
private void configureNamedCommands() {
|
||||
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
||||
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
|
||||
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
|
||||
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
|
||||
}
|
||||
|
||||
//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);
|
||||
|
||||
@@ -286,22 +246,12 @@ public class RobotContainer {
|
||||
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
|
||||
.withWidget(BuiltInWidgets.kGraph);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
|
||||
sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
|
||||
.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)
|
||||
sensorTab.addDouble("Elevator error", elevator::getPIDError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
@@ -310,23 +260,6 @@ public class RobotContainer {
|
||||
|
||||
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
|
||||
|
||||
sensorTab.addDouble("dt distance", drivetrain::driveDistance);
|
||||
|
||||
sensorTab.addDouble("velocity", drivetrain::getVelocity);
|
||||
|
||||
|
||||
//sensorTab.add("odometry", drivetrain::getPose);
|
||||
|
||||
apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag());
|
||||
apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX());
|
||||
apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY());
|
||||
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
|
||||
apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
|
||||
apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
|
||||
apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
|
||||
|
||||
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -341,7 +274,7 @@ public class RobotContainer {
|
||||
@SuppressWarnings("unused")
|
||||
private Command coralIntakeRoutine() {
|
||||
return moveManipulator(
|
||||
ElevatorConstants.kCoralIntakePosition,
|
||||
ElevatorPositions.kCoralIntake,
|
||||
ManipulatorPivotConstants.kCoralIntakePosition
|
||||
)
|
||||
.andThen(manipulator.runUntilCollected(() -> .5));
|
||||
@@ -356,7 +289,7 @@ public class RobotContainer {
|
||||
@SuppressWarnings("unused")
|
||||
private Command algaeIntakeRoutine(boolean l2) {
|
||||
return moveManipulator(
|
||||
l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition,
|
||||
l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae,
|
||||
l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
|
||||
)
|
||||
.andThen(manipulator.runUntilCollected(() -> 1));
|
||||
@@ -369,10 +302,10 @@ public class RobotContainer {
|
||||
* @param armPosition The target rotation of the arm
|
||||
* @return Moves the elevator and arm to the setpoints using the most efficient path
|
||||
*/
|
||||
private Command moveManipulator(double elevatorPosition, double armPosition) {
|
||||
private Command moveManipulator(ElevatorPositions 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.getPosition())) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.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
|
||||
@@ -401,7 +334,7 @@ public class RobotContainer {
|
||||
* @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
|
||||
* @return Moves the elevator and arm to the setpoints
|
||||
*/
|
||||
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
|
||||
private Command moveManipulatorUtil(ElevatorPositions elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
|
||||
|
||||
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
|
||||
armPosition = MathUtil.clamp(
|
||||
@@ -413,13 +346,16 @@ public class RobotContainer {
|
||||
|
||||
return Commands.either(
|
||||
Commands.either(
|
||||
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
elevator.setTargetPosition(elevatorPosition).andThen(
|
||||
new WaitUntilCommand(elevator::atSetpoint),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition)
|
||||
),
|
||||
elevator.setTargetPosition(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
|
||||
() -> sequential
|
||||
),
|
||||
Commands.either(
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.setTargetPosition(elevatorPosition)),
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)),
|
||||
() -> sequential
|
||||
),
|
||||
() -> elevatorFirst
|
||||
@@ -427,18 +363,24 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
|
||||
private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){
|
||||
if(!isL4){
|
||||
return Commands.sequence(
|
||||
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
|
||||
elevator.goToSetpoint(() -> elevatorPosition),
|
||||
elevator.setTargetPosition(elevatorPosition),
|
||||
new WaitUntilCommand(elevator::atSetpoint),
|
||||
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));
|
||||
elevator.setTargetPosition(elevatorPosition),
|
||||
new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()),
|
||||
Commands.parallel(
|
||||
manipulatorPivot.goToSetpoint(() -> armPosition),
|
||||
elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint))
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -450,27 +392,15 @@ public class RobotContainer {
|
||||
* @param armPosition The target rotation of the arm
|
||||
* @return Moves the elevator and arm to the setpoints
|
||||
*/
|
||||
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
|
||||
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
|
||||
elevator.maintainPosition());*/
|
||||
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
|
||||
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
}
|
||||
|
||||
private Command moveWithAlgae(double elevatorPosition, double armPosition) {
|
||||
/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
|
||||
elevator.maintainPosition());*/
|
||||
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
|
||||
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
@SuppressWarnings("unused")
|
||||
private Command safeMoveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
|
||||
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
|
||||
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
|
||||
}
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private Command startingConfig() {
|
||||
return moveManipulatorUtil(0, 0, false, true)
|
||||
return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true)
|
||||
.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
|
||||
}
|
||||
}
|
||||
@@ -12,13 +12,13 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
public class AutoConstants {
|
||||
public static final double kMaxSpeedMetersPerSecond = 4;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 2;
|
||||
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
public static final double kPXController = 6;
|
||||
public static final double kPYController = 6;
|
||||
public static final double kPThetaController = 5.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(
|
||||
|
||||
@@ -13,7 +13,7 @@ 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;
|
||||
public static final double kMaxSpeedMetersPerSecond = 4.8;
|
||||
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
|
||||
|
||||
// Chassis configuration
|
||||
@@ -22,13 +22,6 @@ public class DrivetrainConstants {
|
||||
public static final double kWheelBase = Units.inchesToMeters(24.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;
|
||||
@@ -57,12 +50,6 @@ public class DrivetrainConstants {
|
||||
|
||||
public static final boolean kGyroReversed = true;
|
||||
|
||||
public static final double kHeadingP = 0.01;
|
||||
|
||||
public static final double kXTranslationP = 0.5;
|
||||
public static final double kYTranslationP = 0.5;
|
||||
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
// Distance between front and back wheels on robot
|
||||
|
||||
@@ -11,6 +11,34 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
|
||||
public class ElevatorConstants {
|
||||
public enum ElevatorControlMode {
|
||||
kPID,
|
||||
kManualMaintain
|
||||
}
|
||||
|
||||
public enum ElevatorPositions {
|
||||
kStartingPosition(0),
|
||||
kCoralIntake(0),
|
||||
kL1(0),
|
||||
kL2(14.5),
|
||||
kL2Algae(22),
|
||||
kL3(29),
|
||||
kL3Algae(39),
|
||||
kL4(53),
|
||||
kL4Transition(40),
|
||||
kProcessor(4);
|
||||
|
||||
private double position;
|
||||
|
||||
private ElevatorPositions(double position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
public double getPosition() {
|
||||
return position;
|
||||
}
|
||||
}
|
||||
|
||||
public static final int kElevatorMotor1ID = 8;
|
||||
public static final int kElevatorMotor2ID = 6;
|
||||
|
||||
@@ -29,8 +57,6 @@ public class ElevatorConstants {
|
||||
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 kMaintainP = 3;
|
||||
|
||||
public static final double kAllowedError = 1;
|
||||
|
||||
@@ -38,23 +64,16 @@ public class ElevatorConstants {
|
||||
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 kFeedForwardAlgaeHeldS = 0;
|
||||
public static final double kFeedForwardAlgaeHeldG = 0;
|
||||
public static final double kFeedForwardAlgaeHeldV = 0;
|
||||
|
||||
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 = 0;
|
||||
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 kMaxHeight = 47.5; //actual is 53
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = .25;
|
||||
|
||||
@@ -4,7 +4,9 @@ import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
|
||||
public class ManipulatorConstants {
|
||||
public static final int kManipulatorMotorID = 12;
|
||||
|
||||
public static final int kCoralBeamBreakID = 2;
|
||||
public static final int kAlgaePhotoswitchID = 3;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
}
|
||||
|
||||
@@ -23,33 +23,32 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kPositionalP = 4;
|
||||
public static final double kPositionalI = 0;
|
||||
public static final double kPositionalD = 0;
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
|
||||
|
||||
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 kFFGravityOffset = Units.degreesToRadians(135.0);
|
||||
|
||||
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.78-0.25;
|
||||
public static final double kEncoderOffset = 0.7815;
|
||||
|
||||
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(0.0+90);
|
||||
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);
|
||||
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kL1Position = Units.degreesToRadians(0.0);
|
||||
public static final double kL2Position = Units.degreesToRadians(60.0);
|
||||
public static final double kL3Position = Units.degreesToRadians(60.0);
|
||||
public static final double kL4Position = Units.degreesToRadians(45.0);
|
||||
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
|
||||
public static final double kProcesserPosition = Units.degreesToRadians(175.0);
|
||||
public static final double kNetPosition = Units.degreesToRadians(175.0);
|
||||
/**The closest position to the elevator brace without hitting it */
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
|
||||
public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
|
||||
/**The forward rotation limit of the pivot */
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
|
||||
public static final double kRotationLimit = Units.degreesToRadians(175.0);
|
||||
|
||||
public static final double kSysIDRampRate = 1;
|
||||
public static final double kSysIDStepVolts = 7;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -58,7 +57,6 @@ 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 {
|
||||
@@ -72,8 +70,6 @@ public class ModuleConstants {
|
||||
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,9 +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";
|
||||
}
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
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.
|
||||
@@ -29,36 +28,4 @@ public class VisionConstants {
|
||||
{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.991, 5.246},
|
||||
{5.789, 3.862, 5.789, 4.194},
|
||||
{4.993, 2.816, 5.272, 2.996}
|
||||
};
|
||||
|
||||
public static final double latencyFudge = 0.080;
|
||||
|
||||
}
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
@@ -32,9 +30,9 @@ public class ClimberPivot extends SubsystemBase {
|
||||
neoEncoder = pivotMotor.getEncoder();
|
||||
}
|
||||
|
||||
public Command runPivot(DoubleSupplier speed) {
|
||||
public Command runPivot(double speed) {
|
||||
return run(() -> {
|
||||
pivotMotor.set(speed.getAsDouble());
|
||||
pivotMotor.set(speed);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
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;
|
||||
@@ -34,9 +32,9 @@ public class ClimberRollers extends SubsystemBase {
|
||||
* @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,42 +4,29 @@
|
||||
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.Optional;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
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.PIDController;
|
||||
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.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
|
||||
@@ -53,19 +40,8 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
// 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 PIDController pidHeading;
|
||||
private PIDController pidTranslationX;
|
||||
private PIDController pidTranslationY;
|
||||
|
||||
public Vision vision;
|
||||
|
||||
public Pose2d orangePose2d;
|
||||
private Vision vision;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public Drivetrain() {
|
||||
@@ -97,7 +73,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
m_estimator = new SwerveDrivePoseEstimator(
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(getGyroValue()),
|
||||
Rotation2d.fromDegrees(gyro.getAngle()),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
@@ -107,17 +83,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
new Pose2d()
|
||||
);
|
||||
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
|
||||
|
||||
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
|
||||
pidHeading.enableContinuousInput(-180, 180);
|
||||
|
||||
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
|
||||
pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
|
||||
pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
|
||||
pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
|
||||
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose,
|
||||
this::resetOdometry,
|
||||
@@ -134,24 +99,6 @@ 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();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -166,54 +113,23 @@ public class Drivetrain extends SubsystemBase {
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
|
||||
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
|
||||
|
||||
// if(vision.getOrangeDist() < 72){
|
||||
// m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
|
||||
//}
|
||||
|
||||
// if the detected tags match your alliances reef tags use their pose estimates
|
||||
/*
|
||||
if(vision.getOrangeTagDetected()){
|
||||
// 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());
|
||||
if(orangePose2d.equals(null)){
|
||||
Logger.recordOutput("orange pose", new Pose3d());
|
||||
}else{
|
||||
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
|
||||
}
|
||||
|
||||
Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
|
||||
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
|
||||
|
||||
}else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
|
||||
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
|
||||
}
|
||||
|
||||
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
|
||||
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose));
|
||||
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
|
||||
|
||||
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
|
||||
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose));
|
||||
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
|
||||
if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
|
||||
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
|
||||
}else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
|
||||
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
|
||||
}
|
||||
*/
|
||||
*/
|
||||
|
||||
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
|
||||
if(musicTimer.get()>10){
|
||||
if (m_orchestra.isPlaying()) {
|
||||
m_orchestra.stop();
|
||||
}
|
||||
musicTimer.stop();
|
||||
musicTimer.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -257,20 +173,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()
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
@@ -308,54 +216,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
});
|
||||
}
|
||||
|
||||
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
|
||||
return run(() -> {
|
||||
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
|
||||
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1),
|
||||
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
|
||||
true);
|
||||
|
||||
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
|
||||
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
|
||||
new Rotation2d(headingSetpoint.getAsDouble()))));
|
||||
});
|
||||
}
|
||||
|
||||
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.
|
||||
*/
|
||||
@@ -388,13 +248,9 @@ 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() {
|
||||
gyro.reset();
|
||||
}
|
||||
|
||||
public double getGyroValue() {
|
||||
@@ -410,10 +266,6 @@ public class Drivetrain extends SubsystemBase {
|
||||
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
|
||||
}
|
||||
|
||||
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
|
||||
return gyroBuffer;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
@@ -423,17 +275,7 @@ public class Drivetrain extends SubsystemBase {
|
||||
return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
|
||||
public double driveDistance(){
|
||||
return m_frontLeft.getTotalDist();
|
||||
}
|
||||
|
||||
public double getVelocity(){
|
||||
return m_frontLeft.getState().speedMetersPerSecond;
|
||||
}
|
||||
|
||||
public Command resetToVision(){
|
||||
return run(() -> {
|
||||
m_estimator.resetPose(orangePose2d);
|
||||
});
|
||||
public void addVisionMeasurement(Pose2d pose, double timestamp){
|
||||
m_estimator.addVisionMeasurement(pose, timestamp);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
@@ -15,6 +16,8 @@ import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.ElevatorConstants;
|
||||
import frc.robot.constants.ElevatorConstants.ElevatorControlMode;
|
||||
import frc.robot.constants.ElevatorConstants.ElevatorPositions;
|
||||
|
||||
public class Elevator extends SubsystemBase {
|
||||
protected SparkMax elevatorMotor1;
|
||||
@@ -22,16 +25,20 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
protected RelativeEncoder encoder;
|
||||
|
||||
private BooleanSupplier hasAlgae;
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
|
||||
private PIDController maintainPID;
|
||||
private PIDController pidController;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
private ElevatorFeedforward algaeHeldFeedForward;
|
||||
|
||||
private ElevatorControlMode mode;
|
||||
|
||||
private ElevatorPositions currentTargetPosition;
|
||||
|
||||
public Elevator() {
|
||||
public Elevator(BooleanSupplier hasAlgae) {
|
||||
elevatorMotor1 = new SparkMax(
|
||||
ElevatorConstants.kElevatorMotor1ID,
|
||||
MotorType.kBrushless
|
||||
@@ -56,37 +63,36 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
encoder = elevatorMotor1.getEncoder();
|
||||
|
||||
this.hasAlgae = hasAlgae;
|
||||
|
||||
bottomLimitSwitch = new DigitalInput(
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidControllerDown = new PIDController(
|
||||
pidController = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
|
||||
|
||||
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
|
||||
pidController.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
ElevatorConstants.kFeedForwardG,
|
||||
ElevatorConstants.kFeedForwardV
|
||||
);
|
||||
|
||||
algaeHeldFeedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardAlgaeHeldS,
|
||||
ElevatorConstants.kFeedForwardAlgaeHeldG,
|
||||
ElevatorConstants.kFeedForwardAlgaeHeldV
|
||||
);
|
||||
|
||||
mode = ElevatorControlMode.kPID;
|
||||
|
||||
currentTargetPosition = ElevatorPositions.kCoralIntake;
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -94,6 +100,27 @@ public class Elevator extends SubsystemBase {
|
||||
if (!getBottomLimitSwitch()) {
|
||||
encoder.setPosition(0);
|
||||
}
|
||||
|
||||
if(mode == ElevatorControlMode.kPID) {
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
currentTargetPosition.getPosition()
|
||||
) + (hasAlgae.getAsBoolean() ? algaeHeldFeedForward.calculate(0) : feedForward.calculate(0))
|
||||
);
|
||||
} else {
|
||||
if (hasAlgae.getAsBoolean()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
algaeHeldFeedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -124,7 +151,10 @@ public class Elevator extends SubsystemBase {
|
||||
* @return Sets motor voltage to translate the elevator and maintain position
|
||||
*/
|
||||
public Command runManualElevator(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
return startRun(() -> {
|
||||
mode = ElevatorControlMode.kManualMaintain;
|
||||
},
|
||||
() -> {
|
||||
double desired = speed.getAsDouble();
|
||||
|
||||
if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
|
||||
@@ -132,164 +162,34 @@ public class Elevator extends SubsystemBase {
|
||||
speed.getAsDouble()
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(feedForward.calculate(0));
|
||||
if (hasAlgae.getAsBoolean()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
algaeHeldFeedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 setTargetPosition(ElevatorPositions position) {
|
||||
return runOnce(() -> {
|
||||
if (mode == ElevatorControlMode.kManualMaintain) {
|
||||
pidController.reset();
|
||||
mode = ElevatorControlMode.kPID;
|
||||
}
|
||||
|
||||
currentTargetPosition = position;
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
public Command homeCommand(){
|
||||
return run(() -> {
|
||||
elevatorMotor1.setVoltage(0.5);
|
||||
})
|
||||
.until(() -> elevatorMotor1.getOutputCurrent() > 5)
|
||||
.andThen(run(() -> encoder.setPosition(0)));
|
||||
public boolean atSetpoint() {
|
||||
return pidController.atSetpoint();
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
*
|
||||
* @param setpoint Target destination of the subsystem
|
||||
* @param timeout Time to achieve the setpoint before quitting
|
||||
* @return Sets motor voltage to achieve the target destination
|
||||
*/
|
||||
public Command goToSetpoint(DoubleSupplier setpoint) {
|
||||
|
||||
if (setpoint.getAsDouble() == 0) {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.5)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
|
||||
} else {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
*/
|
||||
/*
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
});*/
|
||||
}
|
||||
|
||||
/*
|
||||
if(encoder.getPosition() >= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerUp.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
||||
);
|
||||
}else if(encoder.getPosition() <= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerDown.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
|
||||
);
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* Returns the current encoder position
|
||||
*
|
||||
@@ -327,19 +227,11 @@ public class Elevator extends SubsystemBase {
|
||||
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
|
||||
}
|
||||
|
||||
public double getPIDUpSetpoint() {
|
||||
return pidControllerUp.getSetpoint();
|
||||
public double getPIDSetpoint() {
|
||||
return pidController.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDUpError() {
|
||||
return pidControllerUp.getError();
|
||||
}
|
||||
|
||||
public double getPIDDownSetpoint() {
|
||||
return pidControllerDown.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDDownError() {
|
||||
return pidControllerDown.getError();
|
||||
public double getPIDError() {
|
||||
return pidController.getError();
|
||||
}
|
||||
}
|
||||
@@ -56,7 +56,6 @@ public class MAXSwerveModule {
|
||||
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);
|
||||
|
||||
m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
|
||||
@@ -75,7 +74,7 @@ public class MAXSwerveModule {
|
||||
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,
|
||||
return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
@@ -87,7 +86,7 @@ public class MAXSwerveModule {
|
||||
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,
|
||||
return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
|
||||
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
|
||||
}
|
||||
|
||||
@@ -108,9 +107,9 @@ public class MAXSwerveModule {
|
||||
// Command driving and turning SPARKS towards their respective setpoints.
|
||||
m_drive.setControl(
|
||||
driveVelocityRequest.withVelocity(
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
).withFeedForward(
|
||||
correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
|
||||
correctedDesiredState.speedMetersPerSecond
|
||||
)
|
||||
);
|
||||
|
||||
@@ -135,16 +134,8 @@ public class MAXSwerveModule {
|
||||
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 getTotalDist(){
|
||||
return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -16,6 +16,7 @@ public class Manipulator extends SubsystemBase {
|
||||
private SparkMax manipulatorMotor;
|
||||
|
||||
private DigitalInput coralBeamBreak;
|
||||
private DigitalInput algaePhotoswitch;
|
||||
|
||||
public Manipulator() {
|
||||
manipulatorMotor = new SparkMax(
|
||||
@@ -30,6 +31,7 @@ public class Manipulator extends SubsystemBase {
|
||||
);
|
||||
|
||||
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
|
||||
algaePhotoswitch = new DigitalInput(ManipulatorConstants.kAlgaePhotoswitchID);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -74,13 +76,6 @@ public class Manipulator extends SubsystemBase {
|
||||
}).until(() -> !coralBeamBreak.get());
|
||||
}
|
||||
|
||||
public Command retractCommand(DoubleSupplier retractSpeed){
|
||||
return run(() -> {
|
||||
manipulatorMotor.set(-retractSpeed.getAsDouble());
|
||||
}
|
||||
).until(() -> coralBeamBreak.get());
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the manipulator in a way that will bring the coral to a reliable holding position
|
||||
*
|
||||
@@ -97,4 +92,8 @@ public class Manipulator extends SubsystemBase {
|
||||
public boolean getCoralBeamBreak() {
|
||||
return coralBeamBreak.get();
|
||||
}
|
||||
|
||||
public boolean getAlgaePhotoswitch() {
|
||||
return algaePhotoswitch.get();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
);
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidController.enableContinuousInput(0, 280);
|
||||
pidController.disableContinuousInput();
|
||||
|
||||
feedForward = new ArmFeedforward(
|
||||
ManipulatorPivotConstants.kFeedForwardS,
|
||||
|
||||
@@ -1,121 +1,94 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
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 DoubleSubscriber black_tx;
|
||||
private DoubleSubscriber black_ty;
|
||||
private DoubleSubscriber black_dist;
|
||||
private DoubleSubscriber blackRobotRelativeX;
|
||||
private DoubleSubscriber blackRobotRelativeY;
|
||||
private DoubleSubscriber blackRobotRelativeZ;
|
||||
|
||||
private DoubleSubscriber blackClosestTag;
|
||||
private BooleanSubscriber blackTagDetected;
|
||||
|
||||
private DoubleSubscriber blackFramerate;
|
||||
|
||||
private DoubleSubscriber orange_tx;
|
||||
private DoubleSubscriber orange_ty;
|
||||
private DoubleSubscriber orange_dist;
|
||||
private DoubleSubscriber orangeRobotRelativeX;
|
||||
private DoubleSubscriber orangeRobotRelativeY;
|
||||
private DoubleSubscriber orangeRobotRelativeZ;
|
||||
|
||||
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};
|
||||
private DoubleSupplier gyroAngle;
|
||||
|
||||
public Vision(){
|
||||
public Vision(DoubleSupplier gyroAngle){
|
||||
NetworkTableInstance inst = NetworkTableInstance.getDefault();
|
||||
|
||||
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
|
||||
NetworkTable 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);
|
||||
blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
|
||||
blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
|
||||
blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").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);
|
||||
orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
|
||||
orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
|
||||
orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").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){
|
||||
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
|
||||
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()));
|
||||
Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
|
||||
|
||||
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()));
|
||||
return new Pose2d(globalPose.getTranslation(), gyroAngle);
|
||||
}
|
||||
|
||||
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(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty));
|
||||
|
||||
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(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(Units.degreesToRadians( camPose[2]))))
|
||||
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(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(){
|
||||
return relativeToGlobalPose2d(getBlackClosestTag(),
|
||||
new Translation2d(getBlackRelativeX(), getBlackRelativeY()),
|
||||
new Rotation2d(gyroAngle.getAsDouble()));
|
||||
}
|
||||
|
||||
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
|
||||
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
|
||||
getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
|
||||
public double getBlackRelativeX(){
|
||||
return blackRobotRelativeX.get();
|
||||
}
|
||||
|
||||
public double getBlackTX(){
|
||||
return black_tx.get();
|
||||
public double getBlackRelativeY(){
|
||||
return blackRobotRelativeY.get();
|
||||
}
|
||||
|
||||
public double getBlackTY(){
|
||||
return black_ty.get();
|
||||
}
|
||||
|
||||
public double getBlackDist(){
|
||||
return black_dist.get();
|
||||
public double getBlackRelativeZ(){
|
||||
return blackRobotRelativeZ.get();
|
||||
}
|
||||
|
||||
public int getBlackClosestTag(){
|
||||
@@ -123,7 +96,7 @@ public class Vision{
|
||||
}
|
||||
|
||||
public double getBlackTimeStamp(){
|
||||
return black_tx.getLastChange();
|
||||
return blackRobotRelativeX.getLastChange();
|
||||
}
|
||||
|
||||
public boolean getBlackTagDetected(){
|
||||
@@ -134,26 +107,22 @@ public class Vision{
|
||||
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(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
|
||||
);
|
||||
}else{
|
||||
return new Pose2d();
|
||||
}
|
||||
public Pose2d getOrangeGlobalPose(){
|
||||
return relativeToGlobalPose2d(getOrangeClosestTag(),
|
||||
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()),
|
||||
new Rotation2d(gyroAngle.getAsDouble()));
|
||||
}
|
||||
|
||||
public double getOrangeTX(){
|
||||
return orange_tx.get();
|
||||
public double getOrangeRelativeX(){
|
||||
return orangeRobotRelativeX.get();
|
||||
}
|
||||
|
||||
public double getOrangeTY(){
|
||||
return orange_ty.get();
|
||||
public double getOrangeRelativeY(){
|
||||
return orangeRobotRelativeY.get();
|
||||
}
|
||||
|
||||
public double getOrangeDist(){
|
||||
return orange_dist.get();
|
||||
public double getOrangeRelativeZ(){
|
||||
return orangeRobotRelativeZ.get();
|
||||
}
|
||||
|
||||
public int getOrangeClosestTag(){
|
||||
@@ -161,7 +130,7 @@ public class Vision{
|
||||
}
|
||||
|
||||
public double getOrangeTimeStamp(){
|
||||
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
|
||||
return orangeRobotRelativeX.getLastChange();
|
||||
}
|
||||
|
||||
public boolean getOrangeTagDetected(){
|
||||
|
||||
@@ -2,6 +2,9 @@ package frc.robot.subsystems.sysid;
|
||||
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import static edu.wpi.first.units.Units.InchesPerSecond;
|
||||
|
||||
import edu.wpi.first.units.measure.MutDistance;
|
||||
@@ -22,8 +25,8 @@ public class ElevatorSysID extends Elevator {
|
||||
|
||||
private SysIdRoutine routine;
|
||||
|
||||
public ElevatorSysID() {
|
||||
super();
|
||||
public ElevatorSysID(BooleanSupplier hasAlgae) {
|
||||
super(hasAlgae);
|
||||
|
||||
appliedVoltage = Volts.mutable(0);
|
||||
|
||||
@@ -38,7 +41,7 @@ public class ElevatorSysID extends Elevator {
|
||||
(log) -> {
|
||||
log.motor("elevatorMotor1")
|
||||
.voltage(appliedVoltage.mut_replace(
|
||||
elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
|
||||
getMotor1(), Volts
|
||||
))
|
||||
.linearPosition(elevatorPosition.mut_replace(
|
||||
encoder.getPosition(), Inches
|
||||
|
||||
@@ -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": []
|
||||
}
|
||||
Reference in New Issue
Block a user