mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Merge pull request #41 from Team4388/Denver-comp
Just move stuff to master
This commit is contained in:
C4llSqin
+9
-21
@@ -4,10 +4,16 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Blue Cage to Reef"
|
||||
"pathName": "Center Barge to Reef 4 Left"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -16,34 +22,16 @@
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Reef to Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
"name": "lower-algae-removal"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"folder": "1 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+9
-21
@@ -4,10 +4,16 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Red Cage to Reef"
|
||||
"pathName": "Center Barge to Reef 4 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -16,34 +22,16 @@
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Reef to Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
"name": "lower-algae-removal"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"folder": "1 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+42
-12
@@ -4,28 +4,34 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Red Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Reef to Feed"
|
||||
"pathName": "Cage 1 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "align-feed"
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 5 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -37,19 +43,43 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef"
|
||||
"pathName": "Top Feed to Reef 6"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 6 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 2 to Reef 5"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 5 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef 6"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 6 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+42
-18
@@ -4,28 +4,34 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Barge to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Reef to Bottom Feed"
|
||||
"pathName": "Cage 3 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 5 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -37,25 +43,43 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef"
|
||||
"pathName": "Top Feed to Reef 6"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 6 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 4 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 3 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 5 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 3 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 6 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 3 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+36
-18
@@ -4,28 +4,34 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Blue Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Reef to Feed"
|
||||
"pathName": "Center Barge to Reef 4 Right"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 4 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -37,25 +43,37 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef"
|
||||
"pathName": "Top Feed to Reef 6"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 6 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+36
-18
@@ -4,28 +4,34 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Barge to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Reef to Feed"
|
||||
"pathName": "Center Barge to Reef 4 Left"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 4 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -37,25 +43,37 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef"
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"folder": "2 Coral",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 2 to Reef 5"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 5 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef 6"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 6 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef 6"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 6 to Top Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 5 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 3 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "await-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "prepare-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Barge to Reef 4 Left"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 4 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-right-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef 2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Reef 2 to Bottom Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "feed-driveback"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Blue Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral-left-l4"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,61 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Blue Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Reef to Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
+25
-25
@@ -7,49 +7,49 @@
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Blue Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"pathName": "Cage 1 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Reef to Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
"pathName": "Cage 2 to Reef 5"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Feed to Reef"
|
||||
"pathName": "Cage 3 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
"pathName": "Cage 4 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
"pathName": "Cage 5 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Cage 6 to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Barge to Reef 4 Left"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Center Barge to Reef 4 Right"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -1,61 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Top Red Cage to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Reef to Feed"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "grab-coral"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Bottom Feed to Reef"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "april-allign"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "place-coral"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -5,9 +5,9 @@
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"type": "named",
|
||||
"data": {
|
||||
"pathName": "Taxi"
|
||||
"name": "taxi"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3040625,
|
||||
"y": 1.3559375
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.892340424329296,
|
||||
"y": 2.067688275398428
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5221875,
|
||||
"y": 2.3675
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.589485733053583,
|
||||
"y": 1.921600370835535
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 5.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0.0,
|
||||
"rotation": 59.28109573597083
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Bottom Feed to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 53.426969021480645
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
+11
-11
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.344772727272727,
|
||||
"y": 2.6688636363636364
|
||||
"x": 6.9834375,
|
||||
"y": 7.1084375
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.4715625,
|
||||
"y": 1.432386363636364
|
||||
"x": 6.292598684210526,
|
||||
"y": 6.699835526315789
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1823863636363636,
|
||||
"y": 1.026578282828283
|
||||
"x": 5.590213815789473,
|
||||
"y": 5.622203947368421
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.9229528762543462,
|
||||
"y": 0.9200437072077402
|
||||
"x": 5.846151315789473,
|
||||
"y": 6.024391447368421
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 55.05578949900953
|
||||
"rotation": -118.81079374297312
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 121.42956561483854
|
||||
"rotation": -115.9743939624313
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.0078125,
|
||||
"y": 6.0725
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.902067334527623,
|
||||
"y": 5.78751784036735
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.590213815789473,
|
||||
"y": 5.593338815789473
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.872115174798128,
|
||||
"y": 6.170888760790882
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [
|
||||
{
|
||||
"fieldPosition": {
|
||||
"x": 5.15,
|
||||
"y": 4.85
|
||||
},
|
||||
"rotationOffset": 0.0,
|
||||
"minWaypointRelativePos": 0.0,
|
||||
"maxWaypointRelativePos": 1.0,
|
||||
"name": "Point Towards Zone"
|
||||
}
|
||||
],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -120.52970589993409
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -123.4533094540727
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+10
-10
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.616287878787879,
|
||||
"y": 5.092234848484848
|
||||
"x": 6.9590625,
|
||||
"y": 4.9878124999999995
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 8.269913357759823,
|
||||
"y": 5.035328772529629
|
||||
"x": 6.709257689548163,
|
||||
"y": 4.997689578268469
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.370012626262627,
|
||||
"y": 5.346338383838384
|
||||
"x": 5.619078947368421,
|
||||
"y": 5.593338815789473
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.997649306769947,
|
||||
"y": 5.369630479499745
|
||||
"x": 6.094391447368421,
|
||||
"y": 5.5202138157894725
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -45,10 +45,10 @@
|
||||
"rotation": -121.26879518614867
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 179.34296858150984
|
||||
"rotation": -124.91940201245771
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+11
-11
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.585795454545455,
|
||||
"y": 1.9413510101010092
|
||||
"x": 6.97125,
|
||||
"y": 3.1474999999999995
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.96969696969697,
|
||||
"y": 1.1282196969696958
|
||||
"x": 6.166875,
|
||||
"y": 2.8184375
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.319191919191919,
|
||||
"y": 2.6833333333333327
|
||||
"x": 5.55172697368421,
|
||||
"y": 2.4470394736842103
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.75625,
|
||||
"y": 2.256439393939394
|
||||
"x": 6.3804769736842095,
|
||||
"y": 2.7395394736842107
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 122.87136694597014
|
||||
"rotation": 120.96375653207355
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 179.5295681977034
|
||||
"rotation": 118.9091836511478
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.016603535353535,
|
||||
"y": 1.9718434343434341
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.670197826108406,
|
||||
"y": 2.0396270358261894
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.5805921052631575,
|
||||
"y": 2.4374177631578946
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.9030178398827875,
|
||||
"y": 2.377301864673819
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 1.0,
|
||||
"rotation": 119.99999999999999
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 123.5866498700801
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+11
-11
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3145202020202018,
|
||||
"y": 0.9655934343434341
|
||||
"x": 7.057260101010102,
|
||||
"y": 0.8537878787878788
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 4.39747159090909,
|
||||
"y": 1.3027556818181814
|
||||
"x": 6.315277777777778,
|
||||
"y": 1.5144570707070704
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.3048863636363635,
|
||||
"y": 2.5292613636363637
|
||||
"x": 5.561348684210526,
|
||||
"y": 2.456661184210526
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.893210227272728,
|
||||
"y": 1.9808238636363626
|
||||
"x": 5.883902345826687,
|
||||
"y": 2.038842371079213
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 114.22774531795413
|
||||
"rotation": 120.98817835541992
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 53.446462811652175
|
||||
"rotation": 132.5633517531898
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+9
-9
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.565467171717171,
|
||||
"y": 4.045328282828282
|
||||
"x": 7.1296875,
|
||||
"y": 4.0493749999999995
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.269289772727273,
|
||||
"y": 4.064886363636362
|
||||
"x": 6.833510101010102,
|
||||
"y": 4.06893308080808
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.172414772727272,
|
||||
"y": 4.045328282828282
|
||||
"x": 6.3496875,
|
||||
"y": 3.8421874999999996
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.696091638273898,
|
||||
"y": 4.0462802990619195
|
||||
"x": 6.599301094934041,
|
||||
"y": 3.8282931405651617
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -45,7 +45,7 @@
|
||||
"rotation": -179.96096735022735
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -179.2890804030095
|
||||
+11
-11
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.116818181818182,
|
||||
"y": 6.956647727272727
|
||||
"x": 7.1053125,
|
||||
"y": 4.061562499999999
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.123068181818182,
|
||||
"y": 6.275650252525252
|
||||
"x": 6.809135101010102,
|
||||
"y": 4.081120580808079
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6196875,
|
||||
"y": 5.35122159090909
|
||||
"x": 6.3496875,
|
||||
"y": 4.195625
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.8392584714142375,
|
||||
"y": 5.792423901494171
|
||||
"x": 6.599301094934041,
|
||||
"y": 4.181730640565162
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.239047023115106
|
||||
"rotation": -179.96096735022735
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Barge to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -52.93323259086456
|
||||
"rotation": -179.2890804030095
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.575631313127491,
|
||||
"y": 6.139141414136251
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.3210457316672,
|
||||
"y": 6.004686892287159
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.268371212121212,
|
||||
"y": 5.417487373737374
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.306061170284897,
|
||||
"y": 5.670117895431184
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -120.86136963407526
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -179.99261564513898
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.982954545454545,
|
||||
"y": 4.03497159090909
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.109744318181819,
|
||||
"y": 3.8355397727272718
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.232244318181818,
|
||||
"y": 5.281420454545455
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.1397939460232385,
|
||||
"y": 4.676387369317844
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 5.574119318181818,
|
||||
"y": 5.720170454545454
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.121401515151515,
|
||||
"y": 7.003093434343434
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.32627122293847843,
|
||||
"y": 7.229156472141377
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -53.93780932479241
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 179.1574757392596
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+10
-10
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.575631313131314,
|
||||
"y": 7.287689393939393
|
||||
"x": 4.0,
|
||||
"y": 6.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.599873737373737,
|
||||
"y": 8.029671717171716
|
||||
"x": 4.2495147804709745,
|
||||
"y": 5.984431624152738
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.344772727272727,
|
||||
"y": 5.391107954545454
|
||||
"x": 4.0,
|
||||
"y": 6.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.121161616161616,
|
||||
"y": 5.00487058080808
|
||||
"x": 4.249520880858856,
|
||||
"y": 5.984529705386749
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -122.99770510121631
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5221875,
|
||||
"y": 2.4040625
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.9564891755960816,
|
||||
"y": 2.1114702488693777
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3284375,
|
||||
"y": 1.3803125
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.0502196323692035,
|
||||
"y": 1.7292616486680126
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.5,
|
||||
"maxAcceleration": 5.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0.0,
|
||||
"rotation": 55.37584492005105
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Reef to Bottom Feed",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 58.10920819815426
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.426644736842105,
|
||||
"y": 2.851151315789474
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 4.23151774045974,
|
||||
"y": 2.4081021765718877
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.7415296052631577,
|
||||
"y": 1.5618421052631584
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.369389020277641,
|
||||
"y": 1.8741228567589594
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 5.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0.0,
|
||||
"rotation": 55.05578949900953
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Reef to Bottom Feed",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 121.42956561483854
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
+13
-13
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.092642045454545,
|
||||
"y": 3.9651704545454542
|
||||
"x": 5.9750822368421055,
|
||||
"y": 3.9865131578947364
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 9.024289772727268,
|
||||
"y": 3.446647727272728
|
||||
"x": 7.0046052631578934,
|
||||
"y": 1.6388157894736843
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.3755050505050503,
|
||||
"y": 0.8842803030303026
|
||||
"x": 1.3040625,
|
||||
"y": 1.368125
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.5707222528658078,
|
||||
"y": 1.040454064918909
|
||||
"x": 1.7992598684210526,
|
||||
"y": 2.302713815789474
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,22 +33,22 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 5.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"velocity": 0.0,
|
||||
"rotation": 52.857102599919905
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Reef to Bottom Feed",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -179.46454101443553
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
+11
-11
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.565467171717171,
|
||||
"y": 0.8334595959595947
|
||||
"x": 5.946217105263157,
|
||||
"y": 4.178947368421053
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 8.123574009981496,
|
||||
"y": 0.3592362071562901
|
||||
"x": 6.604342105263157,
|
||||
"y": 6.518947368421053
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.3903409090909085,
|
||||
"y": 2.6833333333333327
|
||||
"x": 1.452878289473684,
|
||||
"y": 6.565131578947369
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.06636861749931,
|
||||
"y": 2.9361778277713553
|
||||
"x": 2.0499142743221688,
|
||||
"y": 5.561413144603935
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -57.4259428654274
|
||||
"rotation": -53.93780932479242
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Reef to Top Feed",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -0.9548412538721401
|
||||
"rotation": 178.9390883097358
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+12
-12
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.284943181818181,
|
||||
"y": 5.401079545454545
|
||||
"x": 5.41702302631579,
|
||||
"y": 5.256578947368421
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 4.844554924242423,
|
||||
"y": 6.744466540404039
|
||||
"x": 4.4044191919191915,
|
||||
"y": 5.9460227272727275
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.2230429292929292,
|
||||
"y": 7.003093434343434
|
||||
"x": 1.5076388888888888,
|
||||
"y": 6.484722222222222
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.9644034090909093,
|
||||
"y": 5.750085227272726
|
||||
"x": 3.184818220656944,
|
||||
"y": 6.004734862812045
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 5.5,
|
||||
"maxAcceleration": 5.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -45,10 +45,10 @@
|
||||
"rotation": -54.83470564502973
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Reef to Top Feed",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -121.0370223454415
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5099747474747476,
|
||||
"y": 5.742739898989899
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.36577546257746,
|
||||
"y": 6.2120596924932325
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.4143914473684207,
|
||||
"y": 6.584375
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.7066049155713703,
|
||||
"y": 6.056626627340928
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.5,
|
||||
"maxAcceleration": 5.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -53.93780932479242
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Reef to Top Feed",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -58.96173664449721
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.565467171717171,
|
||||
"y": 7.348674242424242
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.8906231049309685,
|
||||
"y": 7.339915637757156
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.305113636363636,
|
||||
"y": 7.348674242424242
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.928529040722133,
|
||||
"y": 7.353679406601922
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 6.681698232005139,
|
||||
"y": 7.343669078246562
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.8889204545454543,
|
||||
"y": 7.348674242424242
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.138853417354843,
|
||||
"y": 7.3544633789160185
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 179.59913485447024
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -179.85896618052556
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.31625,
|
||||
"y": 6.730625
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.5451175614850996,
|
||||
"y": 6.277541643530879
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.540467171717172,
|
||||
"y": 5.7630681818181815
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.590802656557517,
|
||||
"y": 6.105344244914677
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 5.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.239047023115106
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Top Feed to Reef",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -52.93323259086456
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.595959595959596,
|
||||
"y": 3.0289141414141416
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.319147727272727,
|
||||
"y": 2.3198579545454536
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.349684343434343,
|
||||
"y": 2.6934974747474745
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.401761363636364,
|
||||
"y": 1.5819602272727276
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 123.49004753500587
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": -178.9832812445108
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -2,18 +2,28 @@
|
||||
"robotWidth": 0.9,
|
||||
"robotLength": 0.9,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"autoFolders": [],
|
||||
"pathFolders": [
|
||||
"Barge to Reef",
|
||||
"Bottom Feed to Reef",
|
||||
"Reef to Top Feed",
|
||||
"Reef to Bottom Feed",
|
||||
"Top Feed to Reef"
|
||||
],
|
||||
"autoFolders": [
|
||||
"1 Coral",
|
||||
"2 Coral",
|
||||
"3 Coral"
|
||||
],
|
||||
"defaultMaxVel": 3.0,
|
||||
"defaultMaxAccel": 3.0,
|
||||
"defaultMaxAngVel": 540.0,
|
||||
"defaultMaxAngAccel": 720.0,
|
||||
"defaultNominalVoltage": 12.0,
|
||||
"robotMass": 74.088,
|
||||
"robotMass": 58.18,
|
||||
"robotMOI": 6.883,
|
||||
"robotTrackwidth": 0.546,
|
||||
"driveWheelRadius": 0.048,
|
||||
"driveGearing": 5.143,
|
||||
"driveWheelRadius": 0.0508,
|
||||
"driveGearing": 6.12,
|
||||
"maxDriveSpeed": 5.45,
|
||||
"driveMotorType": "krakenX60",
|
||||
"driveCurrentLimit": 60.0,
|
||||
|
||||
@@ -21,7 +21,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
@@ -55,6 +54,7 @@ import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.ReefPositionHelper;
|
||||
import frc4388.utility.Trim;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
@@ -69,7 +69,7 @@ public final class Constants {
|
||||
|
||||
public static final class SwerveDriveConstants {
|
||||
|
||||
public static final double MAX_ROT_SPEED = 3.5;
|
||||
public static final double MAX_ROT_SPEED = Math.PI * 2;
|
||||
public static final double AUTO_MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 1.0;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
@@ -114,7 +114,7 @@ public final class Constants {
|
||||
|
||||
private static final class ModuleSpecificConstants { //2025
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.4794921875+0.25);
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
@@ -122,7 +122,7 @@ public final class Constants {
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5);
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
@@ -130,7 +130,7 @@ public final class Constants {
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(-0.867431640625+0.25);
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
@@ -138,7 +138,7 @@ public final class Constants {
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.425537109375-0.25+0.25);
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
@@ -146,40 +146,6 @@ public final class Constants {
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
}
|
||||
|
||||
/* private static final class ModuleSpecificConstants { // 2024
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
|
||||
} */
|
||||
|
||||
public static final class IDs {
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
|
||||
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5);
|
||||
@@ -228,7 +194,7 @@ public final class Constants {
|
||||
.withKS(0).withKV(0.124);
|
||||
|
||||
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
|
||||
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
|
||||
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
@@ -322,43 +288,76 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class LiDARConstants {
|
||||
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
|
||||
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
|
||||
|
||||
public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
|
||||
|
||||
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
|
||||
public static final int LIDAR_DIO_CHANNEL = 7;
|
||||
public static final int LIDAR_MICROS_TO_CM = 10;
|
||||
public static final int SECONDS_TO_MICROS = 1000000;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
|
||||
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
|
||||
// public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
|
||||
public static final Gains XY_GAINS = new Gains(8,0,0.0);
|
||||
// public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
|
||||
// public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
|
||||
// public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
|
||||
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
|
||||
|
||||
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
|
||||
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
|
||||
|
||||
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
|
||||
// public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
|
||||
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
|
||||
public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
|
||||
public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
|
||||
|
||||
public static final double XY_TOLERANCE = 0.07; // Meters
|
||||
public static final double ROT_TOLERANCE = 5; // Degrees
|
||||
|
||||
public static final double MIN_XY_PID_OUTPUT = 0.0;
|
||||
public static final double MIN_ROT_PID_OUTPUT = 0.0;
|
||||
|
||||
public static final double VELOCITY_THRESHHOLD = 0.01;
|
||||
|
||||
// X is tangent to reef side
|
||||
// Y is normal to reef side
|
||||
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
|
||||
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
|
||||
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
|
||||
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
|
||||
public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
|
||||
|
||||
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
|
||||
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
|
||||
public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
|
||||
// public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
|
||||
|
||||
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
|
||||
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
|
||||
public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
|
||||
|
||||
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
|
||||
public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
|
||||
public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
|
||||
|
||||
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
|
||||
public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
|
||||
|
||||
// public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
|
||||
// public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
|
||||
// // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
|
||||
|
||||
// public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
|
||||
// public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
|
||||
|
||||
// public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
|
||||
// public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
|
||||
|
||||
// public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
|
||||
|
||||
public static final int L4_DRIVE_TIME = 250; //Milliseconds
|
||||
// public static final int L3_DRIVE_TIME = 500;
|
||||
public static final int L2_DRIVE_TIME = 250; //Milliseconds
|
||||
public static final int ALGAE_DRIVE_TIME = 500;
|
||||
public static final double STOP_VELOCITY = 0.0;
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
@@ -368,14 +367,14 @@ public final class Constants {
|
||||
public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
|
||||
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
|
||||
|
||||
public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
|
||||
public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
|
||||
|
||||
// Photonvision thing
|
||||
// The standard deviations of our vision estimated poses, which affect correction rate
|
||||
// X, Y, Theta
|
||||
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
|
||||
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
|
||||
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
|
||||
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
|
||||
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
|
||||
}
|
||||
|
||||
public static final class FieldConstants {
|
||||
@@ -403,6 +402,7 @@ public final class Constants {
|
||||
public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
|
||||
public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
|
||||
public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
|
||||
public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
|
||||
|
||||
public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
|
||||
public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
|
||||
@@ -416,39 +416,47 @@ public final class Constants {
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
|
||||
}
|
||||
public static final class ClimberConstants {
|
||||
public static final CanDevice CLIMBER_ID = new CanDevice("Climber", 311);
|
||||
public static final double CLIMBER_SPEED = 0.1;
|
||||
}
|
||||
|
||||
public static final class ElevatorConstants {
|
||||
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
|
||||
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
|
||||
|
||||
public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
|
||||
public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
|
||||
|
||||
// public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
|
||||
|
||||
public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
|
||||
public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
|
||||
public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
|
||||
|
||||
|
||||
public static final double GEAR_RATIO_ELEVATOR = -9.0;
|
||||
//Max for elevator = 50%
|
||||
|
||||
public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
|
||||
public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
|
||||
public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
|
||||
public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
|
||||
public static final double SCORING_THREE_ELEVATOR = -9.25;
|
||||
public static final double DEALGAE_L2_ELEVATOR = -23.5;
|
||||
public static final double DEALGAE_L3_ELEVATOR = -33.75;
|
||||
public static final double DEALGAE_L2_ELEVATOR = -4;
|
||||
public static final double DEALGAE_L3_ELEVATOR = -26.5;
|
||||
public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
|
||||
|
||||
public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
|
||||
public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
|
||||
//Max for endefector = 60%
|
||||
public static final double L2_SCORE_ELEVATOR = -5;
|
||||
public static final double L2_LEAVE_ELEVATOR = -11;
|
||||
|
||||
public static final double L2_SCORE_ENDEFFECTOR = -19;
|
||||
|
||||
public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
|
||||
public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
|
||||
public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
|
||||
|
||||
public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
|
||||
|
||||
@@ -14,6 +14,7 @@ import java.io.File;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
@@ -27,6 +28,7 @@ import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.robot.Constants.FieldConstants;
|
||||
import frc4388.robot.Constants.LiDARConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.Constants.AutoConstants;
|
||||
@@ -36,6 +38,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
// Commands
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
@@ -43,12 +47,16 @@ import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
|
||||
// Autos
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.DriveUntilLiDAR;
|
||||
import frc4388.robot.commands.GotoLastApril;
|
||||
import frc4388.robot.commands.LidarAlign;
|
||||
import frc4388.robot.commands.MoveForTimeCommand;
|
||||
import frc4388.robot.commands.MoveUntilSuply;
|
||||
import frc4388.robot.commands.WhileTrueCommand;
|
||||
import frc4388.robot.commands.waitElevatorRefrence;
|
||||
import frc4388.robot.commands.waitEndefectorRefrence;
|
||||
import frc4388.robot.commands.waitFeedCoral;
|
||||
import frc4388.robot.commands.waitSupplier;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
|
||||
@@ -61,7 +69,6 @@ import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.Elevator.CoordinationState;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
// import frc4388.robot.subsystems.Endeffector;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
@@ -84,15 +91,16 @@ import frc4388.utility.configurable.ConfigurableString;
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final LED m_robotLED = new LED();
|
||||
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
|
||||
public final Lidar m_lidar = new Lidar();
|
||||
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);
|
||||
public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
|
||||
public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
|
||||
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||
public final Climber m_robotClimber = new Climber(m_robotMap.climber);
|
||||
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
||||
|
||||
|
||||
@@ -110,8 +118,9 @@ public class RobotContainer {
|
||||
|
||||
// ! Teleop Commands
|
||||
public void stop() {
|
||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;;
|
||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||
m_robotSwerveDrive.stopModules();
|
||||
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
||||
}
|
||||
|
||||
// ! /* Autos */
|
||||
@@ -119,18 +128,23 @@ public class RobotContainer {
|
||||
private SendableChooser<String> autoChooser;
|
||||
private Command autoCommand;
|
||||
|
||||
private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
|
||||
// private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
|
||||
// private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed);
|
||||
private Command waitDebuger = new waitSupplier(() -> true);
|
||||
|
||||
// private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
|
||||
// private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
// () -> autoplaybackName.get(), // lastAutoName
|
||||
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
// true, false);
|
||||
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
|
||||
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
|
||||
private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
@@ -139,8 +153,18 @@ public class RobotContainer {
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
new ParallelRaceGroup(
|
||||
new WaitCommand(1),
|
||||
new MoveUntilSuply(
|
||||
m_robotSwerveDrive,
|
||||
new Translation2d(0,-0.5),
|
||||
new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
|
||||
),
|
||||
new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
@@ -148,8 +172,92 @@ public class RobotContainer {
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
// new ConditionalCommand(
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
// () -> m_robotElevator.hasCoral())
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL4RightSemiAuto = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
// waitDebuger.asProxy(),
|
||||
// new ParallelRaceGroup(
|
||||
// new WaitCommand(1),
|
||||
// new MoveUntilSuply(
|
||||
// m_robotSwerveDrive,
|
||||
// new Translation2d(0,-0.5),
|
||||
// new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
|
||||
// ),
|
||||
// new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
|
||||
// () -> m_robotElevator.hasCoral())
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command WannaSeeMeDunk = new SequentialCommandGroup(
|
||||
new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new MoveForTimeCommand(
|
||||
m_robotSwerveDrive,
|
||||
new Translation2d(0,1),
|
||||
new Translation2d(),
|
||||
AutoConstants.L4_DRIVE_TIME,
|
||||
true
|
||||
),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive)
|
||||
);
|
||||
|
||||
/* private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
new ConditionalCommand(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
() -> m_robotElevator.hasCoral()),
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
@@ -160,7 +268,7 @@ public class RobotContainer {
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
@@ -169,19 +277,118 @@ public class RobotContainer {
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
new ConditionalCommand(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
() -> m_robotElevator.hasCoral()),
|
||||
|
||||
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
); */
|
||||
|
||||
private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
new ParallelRaceGroup(
|
||||
new WaitCommand(1),
|
||||
new MoveUntilSuply(
|
||||
m_robotSwerveDrive,
|
||||
new Translation2d(0,-0.5),
|
||||
new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
|
||||
),
|
||||
new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
// // new ConditionalCommand(
|
||||
// // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
// () -> m_robotElevator.hasCoral()),
|
||||
|
||||
|
||||
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command AprilLidarAlignL4LeftSemiAuto = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
|
||||
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
|
||||
), () -> m_robotElevator.isL4Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
// waitDebuger.asProxy(),
|
||||
// new ParallelRaceGroup(
|
||||
// new WaitCommand(1),
|
||||
// new MoveUntilSuply(
|
||||
// m_robotSwerveDrive,
|
||||
// new Translation2d(0,-0.5),
|
||||
// new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
|
||||
// ),
|
||||
// new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
|
||||
|
||||
// new waitEndefectorRefrence(m_robotElevator),
|
||||
|
||||
|
||||
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
// new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
|
||||
|
||||
// // // new ConditionalCommand(
|
||||
// // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
// // () -> m_robotElevator.hasCoral()),
|
||||
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
@@ -191,14 +398,16 @@ public class RobotContainer {
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
|
||||
), () -> m_robotElevator.isL3Primed()),
|
||||
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
@@ -214,7 +423,7 @@ public class RobotContainer {
|
||||
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
// new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
|
||||
),() -> m_robotElevator.isL3Primed()),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
|
||||
@@ -223,9 +432,11 @@ public class RobotContainer {
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
|
||||
|
||||
@@ -239,12 +450,18 @@ public class RobotContainer {
|
||||
|
||||
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
@@ -254,75 +471,147 @@ public class RobotContainer {
|
||||
|
||||
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
|
||||
waitDebuger.asProxy(),
|
||||
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command leftAlgaeRemove = new SequentialCommandGroup(
|
||||
// private Command leftAlgaeRemove = new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
|
||||
// new waitEndefectorRefrence(m_robotElevator),
|
||||
// new waitElevatorRefrence(m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
|
||||
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
|
||||
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
// new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
// new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
// );
|
||||
|
||||
private Command lowerAlgaeRemove = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
|
||||
new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command rightAlgaeRemove = new SequentialCommandGroup(
|
||||
// private Command rightAlgaeRemove = new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
|
||||
// new waitEndefectorRefrence(m_robotElevator),
|
||||
// new waitElevatorRefrence(m_robotElevator),
|
||||
// new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
|
||||
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
|
||||
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
// new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
// new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
// );
|
||||
private Command upperAlgaeRemove = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
|
||||
new waitEndefectorRefrence(m_robotElevator),
|
||||
new waitElevatorRefrence(m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
|
||||
waitDebuger.asProxy(),
|
||||
// new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
|
||||
new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true),
|
||||
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
);
|
||||
|
||||
private Command thrustIntake = new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)
|
||||
new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive),
|
||||
new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive)
|
||||
);
|
||||
|
||||
private Boolean operatorManualMode = false;
|
||||
|
||||
// public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral());
|
||||
// public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral());
|
||||
// public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral());
|
||||
// public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral());
|
||||
// public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral());
|
||||
// public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral());
|
||||
|
||||
// public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
|
||||
// public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
|
||||
// public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
|
||||
// public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
|
||||
// public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
|
||||
// public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
NamedCommands.registerCommand("taxi", new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0, -1),
|
||||
new Translation2d(), 1000, true
|
||||
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
|
||||
|
||||
NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy());
|
||||
NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
|
||||
|
||||
NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive,
|
||||
new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
|
||||
// NamedCommands.registerCommand("feed-driveback", Commands.none());
|
||||
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
|
||||
|
||||
NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator));
|
||||
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0, 1),
|
||||
new Translation2d(), 200, true
|
||||
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
|
||||
new Translation2d(), 300, true
|
||||
|
||||
), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
|
||||
new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
|
||||
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto);
|
||||
NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
|
||||
NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
|
||||
|
||||
NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
|
||||
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT),
|
||||
new LidarAlign(m_robotSwerveDrive, m_lidar)
|
||||
|
||||
NamedCommands.registerCommand("lower-algae-removal", lowerAlgaeRemove);
|
||||
NamedCommands.registerCommand("upper-algae-removal", upperAlgaeRemove);
|
||||
|
||||
NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)),
|
||||
// new waitElevatorRefrence(m_robotElevator),
|
||||
// new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour))
|
||||
));
|
||||
|
||||
configureButtonBindings();
|
||||
@@ -417,10 +706,10 @@ public class RobotContainer {
|
||||
|
||||
// While Left Trigger Pressed: Trims
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
|
||||
@@ -428,6 +717,14 @@ public class RobotContainer {
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;}))
|
||||
.onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;}));
|
||||
|
||||
new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod()));
|
||||
|
||||
// While Left Trigger NOT Pressed: Fine Alignment
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8))
|
||||
.whileTrue(new RunCommand(
|
||||
@@ -440,11 +737,16 @@ public class RobotContainer {
|
||||
), m_robotSwerveDrive))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
// .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive,
|
||||
// new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
|
||||
.onTrue(WannaSeeMeDunk.asProxy());
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(thrustIntake.asProxy());
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar));
|
||||
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
@@ -457,10 +759,10 @@ public class RobotContainer {
|
||||
// Button box
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Five)
|
||||
.onTrue(AprilLidarAlignL4Left);
|
||||
.onTrue(AprilLidarAlignL4LeftSemiAuto);
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.One)
|
||||
.onTrue(AprilLidarAlignL4Right);
|
||||
.onTrue(AprilLidarAlignL4RightSemiAuto);
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Six)
|
||||
.onTrue(AprilLidarAlignL3Left);
|
||||
@@ -476,11 +778,11 @@ public class RobotContainer {
|
||||
|
||||
// Lower algae removal
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Eight)
|
||||
.onTrue(leftAlgaeRemove);
|
||||
.onTrue(lowerAlgaeRemove);
|
||||
|
||||
// Upper algae removal
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Four)
|
||||
.onTrue(rightAlgaeRemove);
|
||||
.onTrue(upperAlgaeRemove);
|
||||
|
||||
|
||||
// Cancel button
|
||||
@@ -511,18 +813,18 @@ public class RobotContainer {
|
||||
.onTrue (new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator))
|
||||
.onFalse(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator));
|
||||
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;}));
|
||||
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
|
||||
// new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
|
||||
|
||||
// Auto Scoring
|
||||
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
.onTrue(AprilLidarAlignL4Left);
|
||||
.onTrue(AprilLidarAlignL4LeftSemiAuto);
|
||||
|
||||
new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
.onTrue(AprilLidarAlignL4Right);
|
||||
.onTrue(AprilLidarAlignL4RightSemiAuto);
|
||||
|
||||
new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode)
|
||||
.onTrue(AprilLidarAlignL3Left);
|
||||
@@ -538,20 +840,12 @@ public class RobotContainer {
|
||||
|
||||
//Controller Lower Algae Removal
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode)
|
||||
.onTrue(leftAlgaeRemove);
|
||||
.onTrue(lowerAlgaeRemove);
|
||||
|
||||
//Controller Upper Algae Removal
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode)
|
||||
.onTrue(rightAlgaeRemove);
|
||||
.onTrue(upperAlgaeRemove);
|
||||
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
|
||||
.onTrue(new InstantCommand(() -> m_robotClimber.climberOut()))
|
||||
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimber()));
|
||||
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> m_robotClimber.climberIn()))
|
||||
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimber()));
|
||||
|
||||
// ? /* Programer Buttons (Controller 3)*/
|
||||
|
||||
// * /* Auto Recording */
|
||||
@@ -627,6 +921,8 @@ public class RobotContainer {
|
||||
// File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
|
||||
String[] autos = dir.list();
|
||||
|
||||
if(autos == null) return;
|
||||
|
||||
for (String auto : autos) {
|
||||
if (auto.endsWith(".auto"))
|
||||
autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
|
||||
@@ -634,7 +930,15 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
autoChooser.onChange((filename) -> {
|
||||
autoCommand = new PathPlannerAuto(filename);
|
||||
if (filename.equals("Taxi")) {
|
||||
autoCommand = new SequentialCommandGroup(
|
||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
||||
new Translation2d(0, -1),
|
||||
new Translation2d(), 1000, true
|
||||
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
|
||||
} else {
|
||||
autoCommand = new PathPlannerAuto(filename);
|
||||
}
|
||||
System.out.println("Robot Auto Changed " + filename);
|
||||
});
|
||||
// SmartDashboard.putData(autoChooser);
|
||||
|
||||
@@ -7,16 +7,25 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import org.photonvision.PhotonCamera;
|
||||
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
import frc4388.robot.Constants.ElevatorConstants;
|
||||
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
// import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
// import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
@@ -46,13 +55,14 @@ public class RobotMap {
|
||||
/* Elevator Subsystem */
|
||||
public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
|
||||
public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
|
||||
|
||||
|
||||
public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
|
||||
public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
|
||||
public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
|
||||
|
||||
/* Climber Subsytem */
|
||||
public final TalonFX climber = new TalonFX(ClimberConstants.CLIMBER_ID.id);
|
||||
|
||||
void configureDriveMotorControllers() {}
|
||||
|
||||
void configureDriveMotorControllers() {
|
||||
// endeffector.saf
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.TimesNegativeOne;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class DriveUntilLiDAR extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Lidar m_lidar;
|
||||
private final double mindistance;
|
||||
private final boolean robotRelative;
|
||||
|
||||
public DriveUntilLiDAR(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Lidar lidar,
|
||||
double mindistance,
|
||||
boolean robotRelative) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.m_lidar = lidar;
|
||||
this.mindistance = mindistance;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveFine(leftStick, rightStick, 0.3);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (Math.abs(m_lidar.getDistance()) < mindistance) {
|
||||
swerveDrive.softStop();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -1,5 +1,7 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
@@ -29,25 +31,27 @@ public class GotoLastApril extends Command {
|
||||
|
||||
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
|
||||
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
|
||||
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
|
||||
// private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
|
||||
private Pose2d targetpos;
|
||||
|
||||
SwerveDrive swerveDrive;
|
||||
Vision vision;
|
||||
double distance;
|
||||
Side side;
|
||||
boolean waitVelocityZero;
|
||||
|
||||
/**
|
||||
* Command to drive robot to position.
|
||||
* @param SwerveDrive m_robotSwerveDrive
|
||||
*/
|
||||
|
||||
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
|
||||
public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.vision = vision;
|
||||
this.distance = distance;
|
||||
this.side = side;
|
||||
// addRequirements(swerveDrive);
|
||||
this.waitVelocityZero = waitVelocityZero && false;
|
||||
addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
|
||||
@@ -56,24 +60,43 @@ public class GotoLastApril extends Command {
|
||||
tagRelativeXError = val;
|
||||
}
|
||||
|
||||
Alliance alliance = null;
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
// double kP = AutoConstants.P_XY_GAINS.get();
|
||||
// double kI = AutoConstants.I_XY_GAINS.get();
|
||||
// double kD = AutoConstants.D_XY_GAINS.get();
|
||||
// xPID = new PID(new Gains(
|
||||
// kP,
|
||||
// kI,
|
||||
// kD),
|
||||
// 0);
|
||||
// yPID = new PID(new Gains(
|
||||
// kP,
|
||||
// kI,
|
||||
// kD),
|
||||
// 0);
|
||||
|
||||
// System.out.println("kP: "+kP);
|
||||
// System.out.println("kI: "+kI);
|
||||
// System.out.println("kD: "+kD);
|
||||
xPID.initialize();
|
||||
yPID.initialize();
|
||||
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side,
|
||||
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
|
||||
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
|
||||
Optional<Alliance> a = DriverStation.getAlliance();
|
||||
if(!a.isEmpty())
|
||||
alliance = a.get();
|
||||
this.targetpos = ReefPositionHelper.getNearestPosition(
|
||||
this.vision.getPose2d(),
|
||||
side,
|
||||
Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()),
|
||||
distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
|
||||
);
|
||||
}
|
||||
|
||||
double xerr;
|
||||
double yerr;
|
||||
double roterr;
|
||||
|
||||
double xoutput;
|
||||
double youtput;
|
||||
double rotoutput;
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
|
||||
@@ -85,28 +108,27 @@ public class GotoLastApril extends Command {
|
||||
|
||||
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
|
||||
|
||||
boolean invert = Math.abs(roterr) > 180;
|
||||
|
||||
if(roterr > 180){
|
||||
roterr -= 360;
|
||||
}else if(roterr < -180){
|
||||
roterr += 360;
|
||||
}
|
||||
|
||||
SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
|
||||
SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
|
||||
SmartDashboard.putNumber("Rotational PID error", roterr);
|
||||
// SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
|
||||
// SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
|
||||
// SmartDashboard.putNumber("Rotational PID error", roterr);
|
||||
|
||||
// SmartDashboard.putNumber("PID X Error", xerr);
|
||||
// SmartDashboard.putNumber("PID Y Error", yerr);
|
||||
SmartDashboard.putNumber("PID X Error", xerr);
|
||||
SmartDashboard.putNumber("PID Y Error", yerr);
|
||||
SmartDashboard.putNumber("PID Rot Error", roterr);
|
||||
|
||||
double xoutput = xPID.update(xerr);
|
||||
double youtput = yPID.update(yerr);
|
||||
double rotoutput = rotPID.update(roterr);
|
||||
xoutput = xPID.update(xerr);
|
||||
youtput = yPID.update(yerr);
|
||||
// rotoutput = rotPID.update(roterr);
|
||||
|
||||
xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
|
||||
youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
|
||||
rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
|
||||
// rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
|
||||
|
||||
|
||||
|
||||
@@ -116,24 +138,28 @@ public class GotoLastApril extends Command {
|
||||
// 0,0
|
||||
);
|
||||
|
||||
Translation2d rightStick = new Translation2d(
|
||||
Math.max(Math.min(rotoutput, 1), -1),
|
||||
0
|
||||
);
|
||||
|
||||
|
||||
// Translation2d rightStick = new Translation2d(
|
||||
// Math.max(Math.min(rotoutput, 1), -1),
|
||||
// 0
|
||||
// );
|
||||
|
||||
setTagRelativeXError(
|
||||
new Translation2d(xerr, yerr).getAngle()
|
||||
.rotateBy(targetpos.getRotation())
|
||||
.getCos());
|
||||
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, true);
|
||||
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
|
||||
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
|
||||
boolean finished = (
|
||||
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
|
||||
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
|
||||
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
|
||||
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
|
||||
);
|
||||
// System.out.println(finished);
|
||||
|
||||
if(finished)
|
||||
|
||||
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
|
||||
this.currentFinderTick = 0;
|
||||
this.speed = 0.4; // TODO: find good speed for this
|
||||
this.foundReef = false;
|
||||
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
|
||||
this.headedRight = (GotoLastApril.tagRelativeXError < 0);
|
||||
this.additionalDistance = 0;
|
||||
this.bounces = 0;
|
||||
}
|
||||
@@ -97,7 +97,7 @@ public class LidarAlign extends Command {
|
||||
currentFinderTick = 0;
|
||||
foundReef = false;
|
||||
return false;
|
||||
} else if (bounces == 2) {
|
||||
} else if (bounces >= 3) {
|
||||
swerveDrive.stopModules();
|
||||
return true;
|
||||
} else {
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.time.Instant;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.TimesNegativeOne;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class MoveUntilSuply extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Supplier<Boolean> truth;
|
||||
private final boolean robotRelative;
|
||||
|
||||
public MoveUntilSuply(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Supplier<Boolean> truth,
|
||||
boolean robotRelative) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.leftStick = leftStick;
|
||||
this.rightStick = rightStick;
|
||||
this.truth = truth;
|
||||
this.robotRelative = robotRelative;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
/**
|
||||
* A command composition that runs one of two commands, depending on the value of the given
|
||||
* condition when this command is initialized.
|
||||
*
|
||||
* <p>The rules for command compositions apply: command instances that are passed to it cannot be
|
||||
* added to any other composition or scheduled individually, and the composition requires all
|
||||
* subsystems its components require.
|
||||
*
|
||||
* <p>This class is provided by the NewCommands VendorDep
|
||||
*/
|
||||
public class WhileTrueCommand extends Command {
|
||||
private final Command m_whileTrue;
|
||||
private final BooleanSupplier m_condition;
|
||||
|
||||
/**
|
||||
* Creates a new WhileTrueCommand.
|
||||
*
|
||||
* @param whileTrue the command to run while the condition is true
|
||||
* @param condition the condition to determine which command to run
|
||||
*/
|
||||
@SuppressWarnings("this-escape")
|
||||
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
|
||||
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
|
||||
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
|
||||
|
||||
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
|
||||
|
||||
// addRequirements(whileTrue.getRequirements());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
if(m_condition.getAsBoolean())
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
m_whileTrue.execute();
|
||||
|
||||
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
|
||||
|
||||
if(!m_whileTrue.isFinished())
|
||||
return;
|
||||
|
||||
if(m_condition.getAsBoolean()){
|
||||
m_whileTrue.end(false);
|
||||
m_whileTrue.initialize();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
m_whileTrue.end(interrupted);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return m_whileTrue.runsWhenDisabled();
|
||||
}
|
||||
|
||||
@Override
|
||||
public InterruptionBehavior getInterruptionBehavior() {
|
||||
if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
|
||||
return InterruptionBehavior.kCancelSelf;
|
||||
} else {
|
||||
return InterruptionBehavior.kCancelIncoming;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
super.initSendable(builder);
|
||||
builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
|
||||
builder.addStringProperty(
|
||||
"selected",
|
||||
() -> {
|
||||
if (m_whileTrue == null) {
|
||||
return "null";
|
||||
} else {
|
||||
return m_whileTrue.getName();
|
||||
}
|
||||
},
|
||||
null);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class waitSupplier extends Command {
|
||||
/** Creates a new waitSupplier. */
|
||||
private final Supplier<Boolean> truth;
|
||||
public waitSupplier(Supplier<Boolean> truth) {
|
||||
this.truth = truth;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return truth.get();
|
||||
}
|
||||
}
|
||||
@@ -1,43 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
public class Climber extends SubsystemBase {
|
||||
/** Creates a new Climber. */
|
||||
|
||||
public TalonFX climberMotor;
|
||||
|
||||
public Climber(TalonFX climberTalonFX) {
|
||||
climberMotor = climberTalonFX;
|
||||
climberMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
}
|
||||
|
||||
public void stopClimber(){
|
||||
climberMotor.set(0);
|
||||
}
|
||||
|
||||
|
||||
public void climberOut(){
|
||||
climberMotor.set(ClimberConstants.CLIMBER_SPEED);
|
||||
|
||||
}
|
||||
|
||||
public void climberIn(){
|
||||
climberMotor.set(-ClimberConstants.CLIMBER_SPEED);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
}
|
||||
@@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.units.measure.Current;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
@@ -47,8 +48,12 @@ public class Elevator extends Subsystem {
|
||||
|
||||
private boolean disableAutoIntake = false;
|
||||
|
||||
private boolean seededZeroEndefector = false;
|
||||
private boolean seededZeroElevator = false;
|
||||
|
||||
private DigitalInput basinBeamBreak;
|
||||
private DigitalInput endeffectorLimitSwitch;
|
||||
private DigitalInput intakeIR;
|
||||
|
||||
public enum CoordinationState {
|
||||
Waiting, // for coral into the though
|
||||
@@ -56,6 +61,7 @@ public class Elevator extends Subsystem {
|
||||
Ready, // Has coral in endefector
|
||||
Hovering, // Has coral in endefector
|
||||
L2Score,
|
||||
L2ScoreLeave,
|
||||
PrimedThree, // Arm and elevator Waiting to score in the level 3 position
|
||||
ScoringThree, // Arm and elevator in the level three position
|
||||
PrimedFour, // Arm and elevator Waiting to score in the level 4 position
|
||||
@@ -68,13 +74,15 @@ public class Elevator extends Subsystem {
|
||||
|
||||
private CoordinationState currentState;
|
||||
|
||||
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
|
||||
// public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
|
||||
public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
|
||||
elevatorMotor = elevatorTalonFX;
|
||||
endeffectorMotor = endeffectorTalonFX;
|
||||
this.led = led;
|
||||
|
||||
this.basinBeamBreak = basinLimitSwitch;
|
||||
this.endeffectorLimitSwitch = endeffectorLimitSwitch;
|
||||
this.intakeIR = intakeDigitalInput;
|
||||
|
||||
elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
@@ -104,12 +112,15 @@ public class Elevator extends Subsystem {
|
||||
|
||||
|
||||
public void transitionState(CoordinationState state) {
|
||||
// elevatorMotor.enable();
|
||||
|
||||
|
||||
currentState = state;
|
||||
switch (currentState) {
|
||||
case Waiting: {
|
||||
wait = System.currentTimeMillis() + maxWait;
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
|
||||
led.setMode(LEDConstants.WAITING_PATTERN);
|
||||
break;
|
||||
}
|
||||
@@ -122,78 +133,85 @@ public class Elevator extends Subsystem {
|
||||
}
|
||||
|
||||
case Ready: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case Hovering: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.READY_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case L2Score: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case L2ScoreLeave: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case PrimedFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case ScoringFour: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case PrimedThree: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case ScoringThree: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL2Primed: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL2Go: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL3Primed: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
|
||||
break;
|
||||
}
|
||||
|
||||
case BallRemoverL3Go: {
|
||||
PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
|
||||
PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
|
||||
led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
|
||||
led.setMode(LEDConstants.SCORING_PATTERN);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -237,11 +255,15 @@ public class Elevator extends Subsystem {
|
||||
// }
|
||||
// }
|
||||
|
||||
public boolean getEndeffectorLimit() {
|
||||
return endeffectorLimitSwitch.get();
|
||||
}
|
||||
|
||||
private void periodicWaiting() {
|
||||
if (!basinBeamBreak.get())
|
||||
transitionState(CoordinationState.Ready);
|
||||
if(!endeffectorLimitSwitch.get())
|
||||
transitionState(CoordinationState.Hovering);
|
||||
// if(!endeffectorLimitSwitch.get())
|
||||
// transitionState(CoordinationState.Hovering);
|
||||
}
|
||||
|
||||
// private void periodicWaitingTripped() {
|
||||
@@ -253,7 +275,7 @@ public class Elevator extends Subsystem {
|
||||
if (elevatorAtReference() && !endeffectorLimitSwitch.get())
|
||||
transitionState(CoordinationState.Hovering);
|
||||
if(elevatorAtReference() && endeffectorLimitSwitch.get())
|
||||
transitionState(CoordinationState.Waiting);
|
||||
transitionState(CoordinationState.Hovering);
|
||||
}
|
||||
|
||||
private void periodicScoring() {
|
||||
@@ -288,10 +310,33 @@ public class Elevator extends Subsystem {
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
// double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
|
||||
// double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
|
||||
// double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
|
||||
// double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
|
||||
|
||||
|
||||
// if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
|
||||
// PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
|
||||
// }
|
||||
|
||||
// This method will be called once per scheduler run
|
||||
// SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
|
||||
// SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
|
||||
SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
|
||||
SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
|
||||
SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
|
||||
SmartDashboard.putString("State", currentState.toString());
|
||||
|
||||
if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
|
||||
endeffectorMotor.setPosition(0);
|
||||
seededZeroEndefector = !seededZeroEndefector;
|
||||
}
|
||||
|
||||
if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
|
||||
elevatorMotor.setPosition(0);
|
||||
seededZeroElevator = !seededZeroElevator;
|
||||
}
|
||||
|
||||
if (disableAutoIntake) return;
|
||||
|
||||
@@ -302,21 +347,32 @@ public class Elevator extends Subsystem {
|
||||
} else if (currentState == CoordinationState.Ready) {
|
||||
periodicReady();
|
||||
}
|
||||
|
||||
if(!intakeIR.get()){
|
||||
led.setMode(LEDConstants.DOWN_PATTERN);
|
||||
}
|
||||
|
||||
|
||||
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
|
||||
// periodicScoring();
|
||||
// }
|
||||
}
|
||||
|
||||
public boolean isL4Primed(){
|
||||
public boolean isL4Primed() {
|
||||
return currentState == CoordinationState.PrimedFour;
|
||||
}
|
||||
|
||||
public boolean isL3Primed(){
|
||||
public boolean isL3Primed() {
|
||||
return currentState == CoordinationState.PrimedThree;
|
||||
}
|
||||
|
||||
public boolean hasCoral(){
|
||||
return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
|
||||
public boolean hasCoral() {
|
||||
return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
|
||||
}
|
||||
|
||||
public boolean readyToMove() {
|
||||
return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
|
||||
// return hasCoral();
|
||||
}
|
||||
|
||||
public void armShuffle(){
|
||||
|
||||
@@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel;
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class Lidar extends Subsystem {
|
||||
|
||||
private double distance = -1;
|
||||
Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
|
||||
private Counter LidarPWM;
|
||||
private String name = "Lidar";
|
||||
|
||||
public Lidar() {
|
||||
private double distance = -1;
|
||||
public Lidar(int port, String name) {
|
||||
this.name = name;
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
|
||||
LidarPWM.reset();
|
||||
|
||||
|
||||
subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
sbDistance = subsystemLayout
|
||||
.add("Distance", 0)
|
||||
.withWidget(BuiltInWidgets.kGraph)
|
||||
.getEntry();
|
||||
|
||||
sbWithinDistance = subsystemLayout
|
||||
. add("Within Distance", 0)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -41,23 +59,13 @@ public class Lidar extends Subsystem {
|
||||
return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
||||
}
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
GenericEntry sbDistance = subsystemLayout
|
||||
.add("Distance", 0)
|
||||
.withWidget(BuiltInWidgets.kGraph)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbWithinDistance = subsystemLayout
|
||||
.add("Within Distance", 0)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
ShuffleboardLayout subsystemLayout;
|
||||
GenericEntry sbDistance;
|
||||
GenericEntry sbWithinDistance;
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Lidar";
|
||||
return "Lidar " + name;
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -58,6 +58,8 @@ public class SwerveDrive extends Subsystem {
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = null;
|
||||
|
||||
|
||||
@@ -116,6 +118,7 @@ public class SwerveDrive extends Subsystem {
|
||||
}
|
||||
|
||||
public void setOdoPose(Pose2d pose) {
|
||||
if (pose == null) return;
|
||||
initalPose2d = pose;
|
||||
swerveDriveTrain.resetPose(pose);
|
||||
}
|
||||
@@ -141,7 +144,7 @@ public class SwerveDrive extends Subsystem {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
stopped = false;
|
||||
if (fieldRelative) {
|
||||
@@ -184,10 +187,12 @@ public class SwerveDrive extends Subsystem {
|
||||
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
|
||||
stopped = false;
|
||||
// Create robot-relative speeds.
|
||||
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -211,18 +216,33 @@ public class SwerveDrive extends Subsystem {
|
||||
.withTargetDirection(rightStick.getAngle()));
|
||||
}
|
||||
|
||||
public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
);
|
||||
swerveDriveTrain.setControl(ctrl);
|
||||
}
|
||||
|
||||
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
|
||||
ctrl.HeadingController.setPID(
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
);
|
||||
.withTargetDirection(heading);
|
||||
// ctrl.HeadingController.setPID(
|
||||
// SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
// SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
|
||||
// SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
|
||||
// );
|
||||
swerveDriveTrain.setControl(ctrl);
|
||||
}
|
||||
|
||||
@@ -288,6 +308,7 @@ public class SwerveDrive extends Subsystem {
|
||||
swerveDriveTrain.tareEverything();
|
||||
robotKnowsWhereItIs = false;
|
||||
rotTarget = 0;
|
||||
// vision.resetRotations();
|
||||
}
|
||||
|
||||
|
||||
@@ -313,9 +334,13 @@ public class SwerveDrive extends Subsystem {
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
double time = Vision.getTime();
|
||||
|
||||
double freq = swerveDriveTrain.getOdometryFrequency();
|
||||
|
||||
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
|
||||
Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
|
||||
Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
|
||||
|
||||
vision.setLastOdomPose(curpose);
|
||||
setLastOdomSpeed(curpose, lastPose, freq);
|
||||
|
||||
if (vision.isTag()) {
|
||||
Pose2d pose = vision.getPose2d();
|
||||
@@ -326,7 +351,9 @@ public class SwerveDrive extends Subsystem {
|
||||
rotTarget += deltaAngle;
|
||||
}
|
||||
|
||||
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
|
||||
vision.addVisionMeasurement(swerveDriveTrain);
|
||||
// swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
|
||||
//back to the ~~future~~ *past*
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
@@ -392,11 +419,35 @@ public class SwerveDrive extends Subsystem {
|
||||
setToSlow();
|
||||
}
|
||||
|
||||
public void startTurboPeriod() {
|
||||
tmp_gear_index = gear_index;
|
||||
setToTurbo();
|
||||
}
|
||||
|
||||
public void endSlowPeriod() {
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
|
||||
gear_index = tmp_gear_index;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
|
||||
if(curPose.isPresent() && lastPose.isPresent()){
|
||||
this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
|
||||
|
||||
|
||||
// double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees();
|
||||
|
||||
// if(rotDiff >= 180){
|
||||
// vision.subtractRotation();
|
||||
// }else if(rotDiff <= -180){
|
||||
// vision.addRotation();
|
||||
// }
|
||||
SmartDashboard.putNumber("Speed", lastOdomSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Swerve Drive Controller";
|
||||
|
||||
@@ -9,6 +9,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
|
||||
import java.time.Instant;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
@@ -24,6 +26,9 @@ import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
@@ -45,16 +50,23 @@ public class Vision extends Subsystem {
|
||||
|
||||
private PhotonCamera[] cameras;
|
||||
private PhotonPoseEstimator[] estimators;
|
||||
private List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
private boolean isTagDetected = false;
|
||||
private boolean isTagProcessed = false;
|
||||
private Pose2d lastVisionPose = new Pose2d();
|
||||
|
||||
private double lastLatency = 0;
|
||||
|
||||
public double getLastLatency() {
|
||||
return lastLatency;
|
||||
}
|
||||
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
private Pose2d lastPhysOdomPose = new Pose2d();
|
||||
|
||||
private Matrix<N3, N1> curStdDevs;
|
||||
|
||||
private Field2d field = new Field2d();
|
||||
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
@@ -92,25 +104,42 @@ public class Vision extends Subsystem {
|
||||
photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
|
||||
this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
|
||||
// resetRotations();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
update();
|
||||
field.setRobotPose(getPose2d());
|
||||
// cameras[0].
|
||||
}
|
||||
|
||||
// public int[] rotations;
|
||||
// public Instant[] lastUpdateTimes;
|
||||
|
||||
// public void resetRotations(){
|
||||
// rotations = new int[cameras.length];
|
||||
// lastUpdateTimes = new Instant[cameras.length];
|
||||
// }
|
||||
|
||||
private Instant lastVisionTime = null;
|
||||
|
||||
|
||||
private void update() {
|
||||
isTagProcessed = false;
|
||||
isTagDetected = false;
|
||||
|
||||
Instant now = Instant.now();
|
||||
|
||||
int cams = 0;
|
||||
|
||||
double X = 0;
|
||||
double Y = 0;
|
||||
double Yaw = 0;
|
||||
// double X = 0;
|
||||
// double Y = 0;
|
||||
// double Yaw = 0;
|
||||
double latency = 0;
|
||||
|
||||
// Pose2d pose = null;
|
||||
poses.clear();
|
||||
|
||||
for(int i = 0; i < cameras.length; i++){
|
||||
PhotonCamera camera = cameras[i];
|
||||
@@ -124,6 +153,7 @@ public class Vision extends Subsystem {
|
||||
|
||||
|
||||
var result = results.get(results.size()-1);
|
||||
latency += result.getTimestampSeconds();
|
||||
|
||||
isTagDetected = isTagDetected | result.hasTargets();
|
||||
|
||||
@@ -136,20 +166,40 @@ public class Vision extends Subsystem {
|
||||
// If the tag was failed to be processed
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
continue;
|
||||
|
||||
poses.add(estimatedRobotPose.get());
|
||||
|
||||
// if(pose == null)
|
||||
// pose = estimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
// else
|
||||
// pose = pose.interpolate(pose, 0.5);
|
||||
// X += pose.getX();
|
||||
// Y += pose.getY();
|
||||
|
||||
// if(X > 6)
|
||||
|
||||
// Yaw += (pose.getRotation().getDegrees() + 180) % 360;
|
||||
// cams++;
|
||||
|
||||
Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
X += pose.getX();
|
||||
Y += pose.getY();
|
||||
Yaw += pose.getRotation().getDegrees();
|
||||
cams++;
|
||||
isTagProcessed = true;
|
||||
|
||||
|
||||
}
|
||||
|
||||
if(isTagProcessed){
|
||||
lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams));
|
||||
}
|
||||
// lastLatency = latency / cams;
|
||||
|
||||
// if(isTagProcessed){
|
||||
|
||||
|
||||
// lastVisionPose = pose;
|
||||
// // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
|
||||
// // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
|
||||
|
||||
// // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
|
||||
// // SmartDashboard.putNumber("Rotations", rotations);
|
||||
|
||||
// lastVisionTime = now;
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
@@ -179,66 +229,66 @@ public class Vision extends Subsystem {
|
||||
return visionEst; // Will be empty
|
||||
|
||||
visionEst = estimator.update(change);
|
||||
updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
// updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
|
||||
return visionEst;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
|
||||
* deviations based on number of tags, estimation strategy, and distance from the tags.
|
||||
*
|
||||
* @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
* @param targets All targets in this camera frame
|
||||
*/
|
||||
private void updateEstimationStdDevs(
|
||||
Optional<EstimatedRobotPose> estimatedPose,
|
||||
List<PhotonTrackedTarget> targets,
|
||||
PhotonPoseEstimator estimator) {
|
||||
if (estimatedPose.isEmpty()) {
|
||||
// No pose input. Default to single-tag std devs
|
||||
curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// /**
|
||||
// * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
|
||||
// * deviations based on number of tags, estimation strategy, and distance from the tags.
|
||||
// *
|
||||
// * @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
// * @param targets All targets in this camera frame
|
||||
// */
|
||||
// private void updateEstimationStdDevs(
|
||||
// Optional<EstimatedRobotPose> estimatedPose,
|
||||
// List<PhotonTrackedTarget> targets,
|
||||
// PhotonPoseEstimator estimator) {
|
||||
// if (estimatedPose.isEmpty()) {
|
||||
// // No pose input. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
|
||||
} else {
|
||||
// Pose present. Start running Heuristic
|
||||
var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
int numTags = 0;
|
||||
double avgDist = 0;
|
||||
// } else {
|
||||
// // Pose present. Start running Heuristic
|
||||
// var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// int numTags = 0;
|
||||
// double avgDist = 0;
|
||||
|
||||
// Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
for (var tgt : targets) {
|
||||
var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
if (tagPose.isEmpty()) continue;
|
||||
// // Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
// for (var tgt : targets) {
|
||||
// var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
// if (tagPose.isEmpty()) continue;
|
||||
|
||||
double distance = tagPose
|
||||
.get()
|
||||
.toPose2d()
|
||||
.getTranslation()
|
||||
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
// double distance = tagPose
|
||||
// .get()
|
||||
// .toPose2d()
|
||||
// .getTranslation()
|
||||
// .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
|
||||
if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
numTags++;
|
||||
avgDist += distance;
|
||||
}
|
||||
}
|
||||
// if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
// numTags++;
|
||||
// avgDist += distance;
|
||||
// }
|
||||
// }
|
||||
|
||||
if (numTags == 0) {
|
||||
// No tags visible. Default to single-tag std devs
|
||||
curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
} else {
|
||||
// One or more tags visible, run the full heuristic.
|
||||
avgDist /= numTags;
|
||||
// Decrease std devs if multiple targets are visible
|
||||
if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// Increase std devs based on (average) distance
|
||||
if (numTags == 1 && avgDist > 4)
|
||||
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
curStdDevs = estStdDevs;
|
||||
}
|
||||
}
|
||||
}
|
||||
// if (numTags == 0) {
|
||||
// // No tags visible. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// } else {
|
||||
// // One or more tags visible, run the full heuristic.
|
||||
// avgDist /= numTags;
|
||||
// // Decrease std devs if multiple targets are visible
|
||||
// if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// // Increase std devs based on (average) distance
|
||||
// if (numTags == 1 && avgDist > 4)
|
||||
// estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
// else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
// curStdDevs = estStdDevs;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
/**
|
||||
* Returns the latest standard deviations of the estimated pose from {@link
|
||||
@@ -253,21 +303,23 @@ public class Vision extends Subsystem {
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
public void setLastOdomPose(Optional<Pose2d> pose){
|
||||
if(pose.isPresent())
|
||||
lastPhysOdomPose = pose.get();
|
||||
}
|
||||
|
||||
// public double getLastOdomSpeed(){
|
||||
// return lastOdomSpeed;
|
||||
// }
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(isTagDetected && isTagProcessed)
|
||||
return lastVisionPose;
|
||||
else
|
||||
if(lastPhysOdomPose != null)
|
||||
return lastPhysOdomPose;
|
||||
return new Pose2d();
|
||||
// if(isTagDetected && isTagProcessed)
|
||||
// // return lastVisionPose;
|
||||
// else
|
||||
// return lastPhysOdomPose;
|
||||
}
|
||||
|
||||
public static double getTime() {
|
||||
@@ -279,7 +331,11 @@ public class Vision extends Subsystem {
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
|
||||
for(EstimatedRobotPose pose : poses){
|
||||
drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@ import java.util.Optional;
|
||||
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.util.Units;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import frc4388.robot.Constants.AutoConstants;
|
||||
@@ -14,7 +15,8 @@ public class ReefPositionHelper {
|
||||
public enum Side {
|
||||
LEFT,
|
||||
RIGHT,
|
||||
CENTER
|
||||
CENTER,
|
||||
FAR_LEFT
|
||||
}
|
||||
|
||||
public static final Pose2d[] RED_TAGS = {
|
||||
@@ -56,6 +58,8 @@ public class ReefPositionHelper {
|
||||
minDistance = dist;
|
||||
}
|
||||
}
|
||||
|
||||
System.out.println(minPos.getRotation().getDegrees());
|
||||
|
||||
return minPos;
|
||||
}
|
||||
@@ -81,11 +85,14 @@ public class ReefPositionHelper {
|
||||
switch(side) {
|
||||
case LEFT:
|
||||
return -(AutoConstants.X_SCORING_POSITION_OFFSET);
|
||||
case FAR_LEFT:
|
||||
return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
|
||||
case RIGHT:
|
||||
return (AutoConstants.X_SCORING_POSITION_OFFSET);
|
||||
case CENTER:
|
||||
return 0;
|
||||
}
|
||||
assert false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -81,21 +81,22 @@ public class Trim {
|
||||
}
|
||||
|
||||
public boolean load() {
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
|
||||
double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
currentValue = fileValue;
|
||||
clampModify();
|
||||
modified = false;
|
||||
if (fileValue != currentValue) {
|
||||
System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
|
||||
modified = true;
|
||||
}
|
||||
return true;
|
||||
} catch (Exception e) {
|
||||
// e.printStackTrace();
|
||||
System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
|
||||
return false;
|
||||
}
|
||||
// try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
|
||||
// double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
// currentValue = fileValue;
|
||||
// clampModify();
|
||||
// modified = false;
|
||||
// if (fileValue != currentValue) {
|
||||
// System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
|
||||
// modified = true;
|
||||
// }
|
||||
// return true;
|
||||
// } catch (Exception e) {
|
||||
// // e.printStackTrace();
|
||||
// System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
|
||||
// return false;
|
||||
// }
|
||||
return false;
|
||||
}
|
||||
|
||||
public void dump() {
|
||||
|
||||
Reference in New Issue
Block a user