diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 461215d3..08755383 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,12 +1,12 @@ { - "robotWidth": 0.85, - "robotLength": 0.952, + "robotWidth": 0.66, + "robotLength": 0.66, "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, + "defaultMaxVel": 3.5, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 270.0, "defaultMaxAngAccel": 600.0, - "maxModuleSpeed": 3.0 + "maxModuleSpeed": 3.5 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index e6dec6ed..668114d0 100644 --- a/build.gradle +++ b/build.gradle @@ -2,8 +2,8 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "org.jetbrains.kotlin.jvm" version "1.6.10" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "org.jetbrains.kotlin.jvm" version "1.6.0" + id "edu.wpi.first.GradleRIO" version "2024.3.1" id "com.diffplug.spotless" version "6.3.0" id "com.peterabeles.gversion" version "1.10" id "org.jetbrains.kotlinx.kover" version "0.4.2" @@ -87,6 +87,7 @@ dependencies { implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" + implementation "gov.nist.math:jama:1.0.3" // We need to add the Kotlin stdlib in order to use most Kotlin language features. // compile "org.jetbrains.kotlin:kotlin-stdlib" diff --git a/simgui.json b/simgui.json index bdde3437..c73084f6 100644 --- a/simgui.json +++ b/simgui.json @@ -74,6 +74,9 @@ "Clients": { "open": true }, + "Connections": { + "open": true + }, "Server": { "open": true }, diff --git a/src/main/deploy/pathplanner/paths/3 note auto.path b/src/main/deploy/pathplanner/paths/3 note auto.path new file mode 100644 index 00000000..69c16eb5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 note auto.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4353122681168922, + "y": 6.599687955484906 + }, + "prevControl": null, + "nextControl": { + "x": 2.4089441986763847, + "y": 6.384061887043822 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.7240209790758945, + "y": 6.292790417717031 + }, + "prevControl": { + "x": 3.1076892656431605, + "y": 6.27784919159728 + }, + "nextControl": { + "x": 5.6050847267357975, + "y": 6.300934892375481 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.125444951049996, + "y": 7.452889295278789 + }, + "prevControl": { + "x": 7.130589600518487, + "y": 6.610606330110338 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 note centerline auto.path b/src/main/deploy/pathplanner/paths/4 note centerline auto.path new file mode 100644 index 00000000..bd185ff1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 note centerline auto.path @@ -0,0 +1,159 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7173996760500974, + "y": 6.565508998353678 + }, + "prevControl": null, + "nextControl": { + "x": 3.555119295355076, + "y": 5.634077091436045 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.742647710559476, + "y": 6.944752946475545 + }, + "prevControl": { + "x": 5.205260844050375, + "y": 6.83199871446017 + }, + "nextControl": { + "x": 6.307184491030538, + "y": 7.063203759276289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.011889957811333, + "y": 7.437737927960554 + }, + "prevControl": { + "x": 6.9255550889526285, + "y": 7.401962318832323 + }, + "nextControl": { + "x": 8.41341650196685, + "y": 7.450961158761761 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.534309232539927, + "y": 6.433553662792256 + }, + "prevControl": { + "x": 6.5112943859614205, + "y": 6.918469113918448 + }, + "nextControl": { + "x": 3.973961025110295, + "y": 6.296111303489134 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.182455809721393, + "y": 5.789537970066136 + }, + "prevControl": { + "x": 6.86458841371461, + "y": 6.557616907301846 + }, + "nextControl": { + "x": 8.388072766841134, + "y": 5.669700379136495 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.534309232539927, + "y": 5.789537970066136 + }, + "prevControl": { + "x": 5.898939970481958, + "y": 7.056402536275015 + }, + "nextControl": { + "x": 3.169678494597896, + "y": 4.522673403857257 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.011889957811333, + "y": 4.091051042493323 + }, + "prevControl": { + "x": 7.17446454036676, + "y": 4.223585978625708 + }, + "nextControl": { + "x": 8.56422220208809, + "y": 4.003636310155177 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.2542200299280735, + "y": 5.010933615765413 + }, + "prevControl": { + "x": 6.077102383375072, + "y": 4.143657250497984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -23.431739603226806, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.17646968828584, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path b/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path index 4b8a0e53..828bf1dc 100644 --- a/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path +++ b/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path @@ -150,7 +150,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 3.5, "maxAcceleration": 3.0, "maxAngularVelocity": 270.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/4NoteAuto.path b/src/main/deploy/pathplanner/paths/4NoteAuto.path index 2c01b0e6..53ed9c46 100644 --- a/src/main/deploy/pathplanner/paths/4NoteAuto.path +++ b/src/main/deploy/pathplanner/paths/4NoteAuto.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.4, - "y": 6.975077486323955 + "x": 2.930867001997314, + "y": 7.04927269938452 }, "prevControl": { - "x": 1.8081527395083707, - "y": 6.967733500350662 + "x": 2.339019741505685, + "y": 7.041928713411227 }, "nextControl": { - "x": 2.6463913517236755, - "y": 6.978134853911492 + "x": 3.1772583537209895, + "y": 7.052330066972057 }, "isLocked": false, "linkedName": null @@ -48,15 +48,15 @@ }, { "anchor": { - "x": 2.337981361851478, + "x": 2.8039263987960847, "y": 5.501318674946378 }, "prevControl": { - "x": 1.7239291602064108, + "x": 2.1898741971510174, "y": 5.488891565963109 }, "nextControl": { - "x": 2.7674829050953003, + "x": 3.233427942039907, "y": 5.510010871590367 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/5 note auto.path b/src/main/deploy/pathplanner/paths/5 note auto.path new file mode 100644 index 00000000..1052aca6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/5 note auto.path @@ -0,0 +1,174 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.372246400173331, + "y": 5.503168727065915 + }, + "prevControl": null, + "nextControl": { + "x": 1.4722464001733309, + "y": 5.503168727065915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.579742831207499, + "y": 5.503168727065915 + }, + "prevControl": { + "x": 2.386891044869959, + "y": 5.4501763832187144 + }, + "nextControl": { + "x": 2.817157982273725, + "y": 5.568406314742786 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.185061299310858, + "y": 5.8276242337023145 + }, + "prevControl": { + "x": 7.332942781824299, + "y": 7.15516562986437 + }, + "nextControl": { + "x": 8.549235660955103, + "y": 5.260265901988496 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.287833090649413, + "y": 4.578971949269507 + }, + "prevControl": { + "x": 7.670593484531014, + "y": 4.907635750207566 + }, + "nextControl": { + "x": 6.675591104432043, + "y": 4.053259828839638 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8677107880836137, + "y": 5.503168727065915 + }, + "prevControl": { + "x": 4.893431162084191, + "y": 4.311068422674352 + }, + "nextControl": { + "x": 2.211050917109747, + "y": 5.889601347341001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7506257145606305, + "y": 6.8538342991756664 + }, + "prevControl": { + "x": 2.3037131784119382, + "y": 6.760493523516374 + }, + "nextControl": { + "x": 3.197538250709323, + "y": 6.947175074834959 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.031860431273965, + "y": 4.469421779553715 + }, + "prevControl": { + "x": 1.6795163846116492, + "y": 4.634659262692578 + }, + "nextControl": { + "x": 2.2946682028088725, + "y": 4.346173791886193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7506257145606305, + "y": 4.214564981505974 + }, + "prevControl": { + "x": 2.512091186113183, + "y": 4.298889426819166 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 145.0, + "rotateFast": false + }, + { + "waypointRelativePos": 3.5, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": -135, + "rotateFast": false + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 158.82179346926867, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/6 note auto.path b/src/main/deploy/pathplanner/paths/6 note auto.path new file mode 100644 index 00000000..f2c1860f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/6 note auto.path @@ -0,0 +1,216 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.372246400173331, + "y": 5.503168727065915 + }, + "prevControl": null, + "nextControl": { + "x": 1.4722464001733309, + "y": 5.503168727065915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.579742831207499, + "y": 5.503168727065915 + }, + "prevControl": { + "x": 2.386891044869959, + "y": 5.4501763832187144 + }, + "nextControl": { + "x": 2.817157982273725, + "y": 5.568406314742786 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.175616322776444, + "y": 5.884312410277054 + }, + "prevControl": { + "x": 7.812495776174484, + "y": 7.20245829853923 + }, + "nextControl": { + "x": 8.8501529104397, + "y": 3.43571059517414 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.926040718376118, + "y": 5.503168727065915 + }, + "prevControl": { + "x": 3.941807538600524, + "y": 4.334161681158379 + }, + "nextControl": { + "x": 3.922738540212626, + "y": 5.748003734063003 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.525427123357103, + "y": 4.469421779553715 + }, + "prevControl": { + "x": 3.697721903463435, + "y": 4.801905458755392 + }, + "nextControl": { + "x": 6.407165539044098, + "y": 4.309021935339161 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.067536433385072, + "y": 4.077862321051606 + }, + "prevControl": { + "x": 8.066027461481239, + "y": 4.188876818525716 + }, + "nextControl": { + "x": 8.068401022452568, + "y": 4.014254827362878 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.579742831207499, + "y": 5.503168727065915 + }, + "prevControl": { + "x": 4.605463205208075, + "y": 4.311068422674352 + }, + "nextControl": { + "x": 1.923082960233632, + "y": 5.889601347341001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7506257145606305, + "y": 6.828451725042869 + }, + "prevControl": { + "x": 2.3037131784119382, + "y": 6.735110949383576 + }, + "nextControl": { + "x": 3.197538250709323, + "y": 6.9217925007021615 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.031860431273965, + "y": 4.469421779553715 + }, + "prevControl": { + "x": 1.6795163846116492, + "y": 4.634659262692578 + }, + "nextControl": { + "x": 2.2946682028088725, + "y": 4.346173791886193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7506257145606305, + "y": 4.188095843716378 + }, + "prevControl": { + "x": 2.512091186113183, + "y": 4.27242028902957 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 145.0, + "rotateFast": false + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": 179.76250667813187, + "rotateFast": false + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": 179.39371249942087, + "rotateFast": false + }, + { + "waypointRelativePos": 7.0, + "rotationDegrees": -134.28717883442204, + "rotateFast": false + }, + { + "waypointRelativePos": 6.0, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 158.82179346926867, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path new file mode 100644 index 00000000..bba2a556 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New New Path.path @@ -0,0 +1,148 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3089621423942004, + "y": 5.4781986647748075 + }, + "prevControl": null, + "nextControl": { + "x": 1.6229091042843384, + "y": 5.48937190229401 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7409995452715665, + "y": 6.998745603379912 + }, + "prevControl": { + "x": 2.0362670003996803, + "y": 6.984089962678806 + }, + "nextControl": { + "x": 3.034562142031102, + "y": 7.004850540597156 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.322993497707589, + "y": 5.466619760836704 + }, + "prevControl": { + "x": 1.604508427071607, + "y": 5.480890706362722 + }, + "nextControl": { + "x": 1.041478568343571, + "y": 5.452348815310687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6276701003693836, + "y": 4.1259845362831165 + }, + "prevControl": { + "x": 2.189127149804064, + "y": 4.273515558573289 + }, + "nextControl": { + "x": 2.8676228747231884, + "y": 4.045261592142421 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.322993497707589, + "y": 5.4781986647748075 + }, + "prevControl": { + "x": 2.178972054977776, + "y": 4.851932345493953 + }, + "nextControl": { + "x": 1.2816064461207233, + "y": 5.508478998602156 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7409995452715665, + "y": 5.5787875652377705 + }, + "prevControl": { + "x": 1.9075546295005208, + "y": 5.802956502587014 + }, + "nextControl": { + "x": 2.9694718922080674, + "y": 5.517336109649897 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3680766375760633, + "y": 5.469439187038876 + }, + "prevControl": { + "x": 2.632532046405176, + "y": 5.375315757759842 + }, + "nextControl": { + "x": 1.1577883071627233, + "y": 5.485092612741088 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.06421609994235, + "y": 5.87883546728025 + }, + "prevControl": { + "x": 4.787635103213693, + "y": 7.584219112563609 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 153.73610016460617, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path new file mode 100644 index 00000000..259ff137 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -0,0 +1,138 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5574483039880436, + "y": 4.090575967465593 + }, + "prevControl": null, + "nextControl": { + "x": 1.6147078347397439, + "y": 4.091942736041143 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.56, + "y": 4.09 + }, + "prevControl": { + "x": 1.5582597358409418, + "y": 4.099244230895869 + }, + "nextControl": { + "x": 1.5617402641590583, + "y": 4.0807557691041305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.29, + "y": 0.7756323517566022 + }, + "prevControl": { + "x": 2.969177358427053, + "y": 1.0807780503726523 + }, + "nextControl": { + "x": 13.610822641572945, + "y": 0.47048665314055205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.33, + "y": 1.6710930995646718 + }, + "prevControl": { + "x": 8.93453802132575, + "y": 1.0602547165380793 + }, + "nextControl": { + "x": -0.2745380213257498, + "y": 2.281931482591264 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.29, + "y": 2.4382121440024136 + }, + "prevControl": { + "x": 4.028441759892057, + "y": 2.341554557982525 + }, + "nextControl": { + "x": 12.55155824010794, + "y": 2.534869730022302 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.84, + "y": 4.090575967465593 + }, + "prevControl": { + "x": 8.715193373642315, + "y": 3.2823751118693654 + }, + "nextControl": { + "x": 0.9648066263576851, + "y": 4.898776823061821 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.255788093535099, + "y": 4.115761438915818 + }, + "prevControl": { + "x": 4.610297359946392, + "y": 4.50726913098882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 140.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.99598392896567, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 00000000..20ff4aa8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,148 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6339414278944301, + "y": 4.441863327809411 + }, + "prevControl": null, + "nextControl": { + "x": 0.9400757836165561, + "y": 3.7236046361560327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1662453490199227, + "y": 2.119949920889425 + }, + "prevControl": { + "x": 1.5702884931470682, + "y": 2.647478011067414 + }, + "nextControl": { + "x": 2.607192449555617, + "y": 1.7296331059676016 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.122786572251385, + "y": 0.7708806736334544 + }, + "prevControl": { + "x": 6.458148740154541, + "y": 0.7673101879045194 + }, + "nextControl": { + "x": 9.78742440434823, + "y": 0.7744511593623893 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.923674997090018, + "y": 3.4598268321908874 + }, + "prevControl": { + "x": 4.703108399200016, + "y": 1.6567638347226934 + }, + "nextControl": { + "x": 2.3479802483551975, + "y": 4.043166393002581 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.122786572251385, + "y": 2.4580567329537444 + }, + "prevControl": { + "x": 5.67378958600948, + "y": 0.5760515879792891 + }, + "nextControl": { + "x": 8.47386794303436, + "y": 2.7278557363131455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.923674997090018, + "y": 3.4598268321908874 + }, + "prevControl": { + "x": 6.142504069079759, + "y": 1.494949935712557 + }, + "nextControl": { + "x": -0.295154074899723, + "y": 5.424703728669218 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.267927559437295, + "y": 7.454709797621273 + }, + "prevControl": { + "x": 5.9477362994801215, + "y": 7.601986302713925 + }, + "nextControl": { + "x": 10.588118819394468, + "y": 7.307433292528621 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.267927559437295, + "y": 5.776386186091238 + }, + "prevControl": { + "x": 9.421093310864284, + "y": 7.379476285826124 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.5932905958447, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 120.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/team4099/utils/PolynomialRegression.java b/src/main/java/com/team4099/utils/PolynomialRegression.java new file mode 100644 index 00000000..53984c32 --- /dev/null +++ b/src/main/java/com/team4099/utils/PolynomialRegression.java @@ -0,0 +1,212 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package com.team4099.utils; + +import Jama.Matrix; +import Jama.QRDecomposition; + +// NOTE: This file is available at +// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html + +/** + * The {@code PolynomialRegression} class performs a polynomial regression on an set of N + * data points (yi, xi). That is, it fits a polynomial + * y = β0 + β1 x + β2 + * x2 + ... + βd xd (where + * y is the response variable, x is the predictor variable, and the + * βi are the regression coefficients) that minimizes the sum of squared + * residuals of the multiple regression model. It also computes associated the coefficient of + * determination R2. + * + *

This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is + * neither the fastest nor the most numerically stable way to perform the polynomial regression. + * + * @author Robert Sedgewick + * @author Kevin Wayne + */ +public class PolynomialRegression implements Comparable { + private final String variableName; // name of the predictor variable + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name + * of the predictor variable. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + this(x, y, degree, "n"); + } + + /** + * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + * @param variableName the name of the predictor variable + * @throws IllegalArgumentException if the lengths of the two arrays are not equal + */ + public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { + this.degree = degree; + this.variableName = variableName; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it + // does + while (true) { + + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) break; + + // decrease degree and try again + this.degree--; + } + + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; + + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } + + /** + * Returns a string representation of the polynomial regression model. + * + * @return a string representation of the polynomial regression model, including the best-fit + * polynomial and the coefficient of determination R2 + */ + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) s.append(String.format("%.10f ", beta(j))); + else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); + j--; + } + s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + + // replace "+ -2n" with "- 2n" + return s.toString().replace("+ -", "- "); + } + + /** Compare lexicographically. */ + public int compareTo(PolynomialRegression that) { + double EPSILON = 1E-5; + int maxDegree = Math.max(this.degree(), that.degree()); + for (int j = maxDegree; j >= 0; j--) { + double term1 = 0.0; + double term2 = 0.0; + if (this.degree() >= j) term1 = this.beta(j); + if (that.degree() >= j) term2 = that.beta(j); + if (Math.abs(term1) < EPSILON) term1 = 0.0; + if (Math.abs(term2) < EPSILON) term2 = 0.0; + if (term1 < term2) return -1; + else if (term1 > term2) return +1; + } + return 0; + } + + /** + * Unit tests the {@code PolynomialRegression} data type. + * + * @param args the command-line arguments + */ + public static void main(String[] args) { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + PolynomialRegression regression = new PolynomialRegression(x, y, 3); + + System.out.println(regression); + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt index 2fb749d5..b5d45d2d 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt @@ -1,10 +1,12 @@ package com.team4099.lib.trajectory import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.math.controller.ProfiledPIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.trajectory.Trajectory +import org.team4099.lib.units.derived.radians /** * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain @@ -20,7 +22,7 @@ import edu.wpi.first.math.trajectory.Trajectory class CustomHolonomicDriveController( private val m_xController: PIDController, private val m_yController: PIDController, - private val m_thetaController: PIDController + private val m_thetaController: ProfiledPIDController ) { private var m_poseError = Pose2d() private var m_rotationError = Rotation2d() diff --git a/src/main/kotlin/com/team4099/lib/vision/TimestampedTrigVisionUpdate.kt b/src/main/kotlin/com/team4099/lib/vision/TimestampedTrigVisionUpdate.kt new file mode 100644 index 00000000..17bb1f90 --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/vision/TimestampedTrigVisionUpdate.kt @@ -0,0 +1,9 @@ +package com.team4099.lib.vision + +import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.units.base.Time + +data class TimestampedTrigVisionUpdate( + val timestamp: Time, + val robotTSpeaker: Transform2d, +) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index cfe0e8e8..0066d232 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -8,11 +8,11 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.Alert.AlertType +import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.NTSafePublisher import edu.wpi.first.hal.AllianceStationID import edu.wpi.first.networktables.GenericEntry -import edu.wpi.first.networktables.NetworkTableValue import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.PowerDistribution import edu.wpi.first.wpilibj.RobotBase @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.simulation.DriverStationSim import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler +import edu.wpi.first.wpilibj2.command.WaitCommand import org.ejml.EjmlVersion.BUILD_DATE import org.ejml.EjmlVersion.DIRTY import org.ejml.EjmlVersion.GIT_BRANCH @@ -49,6 +50,8 @@ object Robot : LoggedRobot() { val logTuningModeEnabled = Alert("Tuning Mode Enabled. Expect loop times to be greater", AlertType.WARNING) lateinit var allianceSelected: GenericEntry + lateinit var autonomousCommand: Command + lateinit var autonomousLoadingCommand: Command /* val port0 = AnalogInput(0) val port1 = AnalogInput(1) @@ -96,7 +99,7 @@ object Robot : LoggedRobot() { Constants.Universal.POWER_DISTRIBUTION_HUB_ID, PowerDistribution.ModuleType.kRev ) } else { - when (Constants.Tuning.SimType.SIM) { + when (Constants.Universal.SIM_MODE) { Constants.Tuning.SimType.SIM -> { Logger.addDataReceiver(NTSafePublisher()) logSimulationAlert.set(true) @@ -125,15 +128,15 @@ object Robot : LoggedRobot() { // Set the scheduler to log events for command initialize, interrupt, finish CommandScheduler.getInstance().onCommandInitialize { command: Command -> - Logger.recordOutput("/ActiveCommands/${command.name}", true) + DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", true) } CommandScheduler.getInstance().onCommandFinish { command: Command -> - Logger.recordOutput("/ActiveCommands/${command.name}", false) + DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false) } CommandScheduler.getInstance().onCommandInterrupt { command: Command -> - Logger.recordOutput("/ActiveCommands/${command.name}", false) + DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false) } val autoTab = Shuffleboard.getTab("Pre-match") @@ -143,23 +146,21 @@ object Robot : LoggedRobot() { .withPosition(0, 1) .withWidget(BuiltInWidgets.kTextView) .entry + + RobotContainer.zeroSensors(isInAutonomous = true) } override fun autonomousInit() { - RobotContainer.zeroSensors(isInAutonomous = true) - RobotContainer.setDriveBrakeMode() - RobotContainer.setSteeringBrakeMode() - RobotContainer.getAutonomousCommand().schedule() + val autonCommandWithWait = WaitCommand(0.001).andThen(autonomousCommand) + autonCommandWithWait?.schedule() } override fun disabledPeriodic() { FMSData.allianceColor = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + autonomousCommand = RobotContainer.getAutonomousCommand() } override fun disabledInit() { - RobotContainer.getAutonomousCommand().cancel() - RobotContainer.setSteeringCoastMode() - RobotContainer.setDriveBrakeMode() RobotContainer.requestIdle() // autonomousCommand.cancel() } @@ -184,29 +185,27 @@ object Robot : LoggedRobot() { (Clock.realTimestamp - motorCheckerStartTime).inMilliseconds ) + val superstructureLoopTimeMS = Clock.realTimestamp RobotContainer.superstructure.periodic() + Logger.recordOutput( + "LoggedRobot/Subsystems/SuperstructureLoopTimeMS", + (Clock.realTimestamp - superstructureLoopTimeMS).inMilliseconds + ) Logger.recordOutput( "LoggedRobot/RemainingRamMB", Runtime.getRuntime().freeMemory() / 1024 / 1024 ) - Logger.recordOutput("LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds) + DebugLogger.recordDebugOutput( + "LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds + ) ControlBoard.rumbleConsumer.accept(RobotContainer.rumbleState) - val currentAlliance = - try { - DriverStation.getAlliance().get().toString() - } catch (_: NoSuchElementException) { - "No alliance" - } - - allianceSelected.set(NetworkTableValue.makeString(currentAlliance)) - /* - Logger.recordOutput("LoggedRobot/port0", port0.voltage) - Logger.recordOutput("LoggedRobot/port1", port1.voltage) - Logger.recordOutput("LoggedRobot/port2", port2.voltage) + DebugLogger.recordDebugOutput("LoggedRobot/port0", port0.voltage) + DebugLogger.recordDebugOutput("LoggedRobot/port1", port1.voltage) + DebugLogger.recordDebugOutput("LoggedRobot/port2", port2.voltage) Logger.recordOutput("LoggedRobot/port3", port3.voltage) */ @@ -214,10 +213,11 @@ object Robot : LoggedRobot() { override fun teleopInit() { FMSData.allianceColor = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) + RobotContainer.zeroSensors(isInAutonomous = false) RobotContainer.mapTeleopControls() RobotContainer.getAutonomousCommand().cancel() RobotContainer.setDriveBrakeMode() - RobotContainer.setSteeringBrakeMode() + RobotContainer.setSteeringCoastMode() if (Constants.Tuning.TUNING_MODE) { RobotContainer.mapTunableCommands() } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 386130da..9c96230a 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,8 +1,11 @@ package com.team4099.robot2023 +import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand +import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand +import com.team4099.robot2023.commands.drivetrain.TargetSpeakerCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -21,13 +24,14 @@ import com.team4099.robot2023.subsystems.flywheel.Flywheel import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon import com.team4099.robot2023.subsystems.intake.Intake -import com.team4099.robot2023.subsystems.intake.IntakeIONEO +import com.team4099.robot2023.subsystems.intake.IntakeIOFalconNEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision +import com.team4099.robot2023.subsystems.vision.camera.CameraIO import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIOSim @@ -35,10 +39,12 @@ import com.team4099.robot2023.subsystems.wrist.WristIOTalon import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Commands.runOnce import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import org.team4099.lib.smoothDeadband import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { @@ -54,6 +60,15 @@ object RobotContainer { val rumbleState get() = feeder.rumbleTrigger + var setClimbAngle = -1337.degrees + var setAmpAngle = 270.0.degrees + var climbAngle: () -> Angle = { setClimbAngle } + var ampAngle: () -> Angle = { setAmpAngle } + + val podiumAngle = + LoggedTunableValue( + "Defense/PodiumShotAngle", 25.0.degrees, Pair({ it.inDegrees }, { it.degrees }) + ) init { if (RobotBase.isReal()) { @@ -61,9 +76,9 @@ object RobotContainer { // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) - vision = Vision(CameraIOPhotonvision("parakeet_1")) + vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2")) limelight = LimelightVision(object : LimelightVisionIO {}) - intake = Intake(IntakeIONEO) + intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) elevator = Elevator(ElevatorIONEO) flywheel = Flywheel(FlywheelIOTalon) @@ -71,7 +86,7 @@ object RobotContainer { } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - vision = Vision(CameraIOPhotonvision("parakeet_1")) + vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2")) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOSim) feeder = Feeder(FeederIOSim) @@ -80,8 +95,13 @@ object RobotContainer { wrist = Wrist(WristIOSim) } - superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain) - vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }) + superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision) + vision.setDataInterfaces( + { drivetrain.fieldTRobot }, + { drivetrain.addVisionData(it) }, + { drivetrain.addSpeakerVisionData(it) } + ) + vision.drivetrainOdometry = { drivetrain.odomTRobot } limelight.poseSupplier = { drivetrain.odomTRobot } } @@ -143,7 +163,21 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) - ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand()) + ControlBoard.intake.whileTrue( + ParallelCommandGroup( + superstructure.groundIntakeCommand(), + TargetNoteCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + limelight + ) + ) + ) + ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand()) ControlBoard.prepHighProtected.whileTrue( @@ -156,7 +190,13 @@ object RobotContainer { { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain, - 30.degrees, + { + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + podiumAngle.get() + else 180.degrees - podiumAngle.get() + }, ) ) ) @@ -168,52 +208,64 @@ object RobotContainer { ControlBoard.prepLow.whileTrue(superstructure.prepSpeakerLowCommand()) ControlBoard.prepTrap.whileTrue(superstructure.prepTrapCommand()) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) + ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand()) ControlBoard.targetAmp.whileTrue( - TargetAngleCommand( - driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, - { ControlBoard.slowMode }, - drivetrain, - 270.0.degrees - ) + runOnce({ + val currentRotation = drivetrain.odomTRobot.rotation + setAmpAngle = + if (currentRotation > 0.0.degrees && currentRotation < 180.degrees) { + 90.degrees + } else { + 270.degrees + } + }) + .andThen( + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + ampAngle + ) + ) ) ControlBoard.climbAlignFar.whileTrue( - TargetAngleCommand( - driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, - { ControlBoard.slowMode }, - drivetrain, - if (DriverStation.getAlliance().isPresent && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red - ) - 0.0.degrees - else 180.0.degrees - ) + runOnce({ + setClimbAngle = + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + 180.degrees + else (0).degrees + }) ) ControlBoard.climbAlignLeft.whileTrue( - TargetAngleCommand( - driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, - { ControlBoard.slowMode }, - drivetrain, - if (DriverStation.getAlliance().isPresent && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red - ) - 120.degrees - else -60.degrees - ) + runOnce({ + setClimbAngle = + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + 300.degrees + else (120).degrees + }) ) ControlBoard.climbAlignRight.whileTrue( + runOnce({ + setClimbAngle = + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + (60).degrees + else 240.degrees + }) + ) + ControlBoard.climbAutoAlign.whileTrue( TargetAngleCommand( driver = Ryan(), { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, @@ -221,13 +273,61 @@ object RobotContainer { { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain, - if (DriverStation.getAlliance().isPresent && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red - ) - -120.0.degrees - else 60.0.degrees, + climbAngle ) ) + // ControlBoard.climbAlignLeft.whileTrue( + // TargetAngleCommand( + // driver = Ryan(), + // { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + // { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + // { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + // { ControlBoard.slowMode }, + // drivetrain, + // if (DriverStation.getAlliance().isPresent && + // DriverStation.getAlliance().get() == DriverStation.Alliance.Red + // ) + // 120.degrees + // else -60.degrees + // ) + // ) + // + // ControlBoard.climbAlignRight.whileTrue( + // TargetAngleCommand( + // driver = Ryan(), + // { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + // { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + // { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + // { ControlBoard.slowMode }, + // drivetrain, + // if (DriverStation.getAlliance().isPresent && + // DriverStation.getAlliance().get() == DriverStation.Alliance.Red + // ) + // -120.0.degrees + // else 60.0.degrees, + // ) + // ) + + ControlBoard.targetSpeaker.whileTrue( + ParallelCommandGroup( + TargetSpeakerCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + vision + ), + superstructure.autoAimCommand() + ) + ) + + // ControlBoard.characterizeSubsystem.whileTrue( + // WheelRadiusCharacterizationCommand( + // drivetrain, WheelRadiusCharacterizationCommand.Companion.Direction.CLOCKWISE + // ) + // ) /* TUNING COMMANDS @@ -257,5 +357,7 @@ object RobotContainer { fun getAutonomousCommand() = AutonomousSelector.getCommand(drivetrain, superstructure) + fun getAutonomousLoadingCommand() = AutonomousSelector.getLoadingCommand(drivetrain) + fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index e1d5522e..9a113d6b 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -1,13 +1,20 @@ package com.team4099.robot2023.auto +import com.team4099.robot2023.auto.mode.ExamplePathAuto +import com.team4099.robot2023.auto.mode.FiveNoteAutoPath import com.team4099.robot2023.auto.mode.FourNoteAutoPath import com.team4099.robot2023.auto.mode.FourNoteLeftCenterLine import com.team4099.robot2023.auto.mode.FourNoteMiddleCenterLine import com.team4099.robot2023.auto.mode.FourNoteRightCenterLine import com.team4099.robot2023.auto.mode.PreloadAndLeaveCenterSubwooferAutoPath -import com.team4099.robot2023.auto.mode.PreloadAndLeaveLeftSubwooferAutoPath -import com.team4099.robot2023.auto.mode.PreloadAndLeaveRightSubwooferAutoPath +import com.team4099.robot2023.auto.mode.PreloadAndLeaveFromAmpSubwooferAutoPath +import com.team4099.robot2023.auto.mode.PreloadAndLeaveFromSourceSubwooferAutoPath +import com.team4099.robot2023.auto.mode.SixNoteAutoPath import com.team4099.robot2023.auto.mode.TestAutoPath +import com.team4099.robot2023.auto.mode.ThreeNoteAndPickupCenterlineSourceAutoPath +import com.team4099.robot2023.auto.mode.ThreeNoteCenterlineFromAmpAutoPath +import com.team4099.robot2023.auto.mode.TwoNoteCenterlineFromAmpAutoPath +import com.team4099.robot2023.auto.mode.TwoNoteCenterlineFromSourceAutoPath import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.util.AllianceFlipUtil @@ -49,17 +56,43 @@ object AutonomousSelector { "Four Note LEFT Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_LEFT_AUTO_PATH ) autonomousModeChooser.addOption( - "Preload + Leave from Left Side of Subwoofer", + "Five Note Auto from Center Subwoofer", AutonomousMode.FIVE_NOTE_AUTO_PATH + ) + autonomousModeChooser.addOption( + "Two Note Centerline Auto from Source Side of Subwoofer", + AutonomousMode.TWO_NOTE_CENTERLINE_FROM_SOURCE + ) + + autonomousModeChooser.addOption( + "Two Note Centerline Auto from Amp Side of Subwoofer", + AutonomousMode.TWO_NOTE_CENTERLINE_FROM_AMP + ) + autonomousModeChooser.addOption( + "Three Note Centerline Auto from Amp Side of Subwoofer", + AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP + ) + autonomousModeChooser.addOption( + "Three Note + Pickup Centerline Auto from Source Side of Subwoofer", + AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE + ) + autonomousModeChooser.addOption( + "Preload + Leave from Amp Side of Subwoofer", AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER ) autonomousModeChooser.addOption( - "Preload + Leave from Right Side of Subwoofer", + "Preload + Leave from Source Side of Subwoofer", AutonomousMode.PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER ) autonomousModeChooser.addOption( "Preload + Leave from Center Side of Subwoofer", AutonomousMode.PRELOAD_AND_LEAVE_CENTER_SUBWOOFER ) + + autonomousModeChooser.addOption("Six Note Path", AutonomousMode.SIX_NOTE_AUTO_PATH) + autonomousModeChooser.addOption( + "Six Note Path with Pickup", AutonomousMode.SIX_NOTE_WITH_PICKUP_PATH + ) + autonomousModeChooser.addOption("Test Auto Path", AutonomousMode.TEST_AUTO_PATH) // autonomousModeChooser.addOption("Characterize Elevator", // AutonomousMode.ELEVATOR_CHARACTERIZE) autoTab.add("Mode", autonomousModeChooser.sendableChooser).withSize(4, 2).withPosition(2, 0) @@ -98,7 +131,16 @@ object AutonomousSelector { AllianceFlipUtil.apply(TestAutoPath.startingPose) ) }) - .andThen(TestAutoPath(drivetrain)) + .andThen(TestAutoPath(drivetrain, superstructure)) + AutonomousMode.SIX_NOTE_AUTO_PATH -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + drivetrain.tempZeroGyroYaw(TestAutoPath.startingPose.rotation) + drivetrain.resetFieldFrameEstimator( + AllianceFlipUtil.apply(TestAutoPath.startingPose) + ) + }) + .andThen(SixNoteAutoPath(drivetrain, superstructure)) AutonomousMode.FOUR_NOTE_AUTO_PATH -> return WaitCommand(waitTime.inSeconds) .andThen({ @@ -131,16 +173,60 @@ object AutonomousSelector { drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen(FourNoteMiddleCenterLine(drivetrain, superstructure)) + AutonomousMode.FIVE_NOTE_AUTO_PATH -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = AllianceFlipUtil.apply(FiveNoteAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(FiveNoteAutoPath(drivetrain, superstructure)) + AutonomousMode.TWO_NOTE_CENTERLINE_FROM_SOURCE -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(TwoNoteCenterlineFromSourceAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(TwoNoteCenterlineFromSourceAutoPath(drivetrain, superstructure)) + AutonomousMode.TWO_NOTE_CENTERLINE_FROM_AMP -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(TwoNoteCenterlineFromAmpAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(TwoNoteCenterlineFromAmpAutoPath(drivetrain, superstructure)) + AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(TwoNoteCenterlineFromAmpAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(ThreeNoteCenterlineFromAmpAutoPath(drivetrain, superstructure)) + AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE -> + return WaitCommand(waitTime.inSeconds) + .andThen({ + val flippedPose = + AllianceFlipUtil.apply(ThreeNoteAndPickupCenterlineSourceAutoPath.startingPose) + drivetrain.tempZeroGyroYaw(flippedPose.rotation) + drivetrain.resetFieldFrameEstimator(flippedPose) + }) + .andThen(ThreeNoteAndPickupCenterlineSourceAutoPath(drivetrain, superstructure)) AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER -> return WaitCommand(waitTime.inSeconds) .andThen({ val flippedPose = - AllianceFlipUtil.apply(PreloadAndLeaveLeftSubwooferAutoPath.startingPose) + AllianceFlipUtil.apply(PreloadAndLeaveFromAmpSubwooferAutoPath.startingPose) drivetrain.tempZeroGyroYaw(flippedPose.rotation) drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen( - PreloadAndLeaveLeftSubwooferAutoPath( + PreloadAndLeaveFromAmpSubwooferAutoPath( drivetrain, superstructure, secondaryWaitTime ) ) @@ -148,12 +234,12 @@ object AutonomousSelector { return WaitCommand(waitTime.inSeconds) .andThen({ val flippedPose = - AllianceFlipUtil.apply(PreloadAndLeaveRightSubwooferAutoPath.startingPose) + AllianceFlipUtil.apply(PreloadAndLeaveFromSourceSubwooferAutoPath.startingPose) drivetrain.tempZeroGyroYaw(flippedPose.rotation) drivetrain.resetFieldFrameEstimator(flippedPose) }) .andThen( - PreloadAndLeaveRightSubwooferAutoPath( + PreloadAndLeaveFromSourceSubwooferAutoPath( drivetrain, superstructure, secondaryWaitTime ) ) @@ -175,14 +261,25 @@ object AutonomousSelector { return InstantCommand() } + fun getLoadingCommand(drivetrain: Drivetrain): Command { + return ExamplePathAuto(drivetrain) + } + private enum class AutonomousMode { TEST_AUTO_PATH, FOUR_NOTE_AUTO_PATH, FOUR_NOTE_RIGHT_AUTO_PATH, FOUR_NOTE_MIDDLE_AUTO_PATH, FOUR_NOTE_LEFT_AUTO_PATH, + TWO_NOTE_CENTERLINE_FROM_SOURCE, + TWO_NOTE_CENTERLINE_FROM_AMP, + THREE_NOTE_CENTERLINE_FROM_AMP, + THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE, PRELOAD_AND_LEAVE_LEFT_SUBWOOFER, PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER, - PRELOAD_AND_LEAVE_CENTER_SUBWOOFER + PRELOAD_AND_LEAVE_CENTER_SUBWOOFER, + FIVE_NOTE_AUTO_PATH, + SIX_NOTE_AUTO_PATH, + SIX_NOTE_WITH_PICKUP_PATH } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt index e0dd96b9..1bdaf786 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt @@ -1,17 +1,40 @@ package com.team4099.robot2023.auto.mode -import com.team4099.robot2023.auto.PathStore -import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() { init { addRequirements(drivetrain) addCommands( - ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ) + ) + } + ) ) } + companion object { + val startingPose = Pose2d(Translation2d(1.42.meters, 5.535.meters), 180.degrees) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt new file mode 100644 index 00000000..fadcd1c8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt @@ -0,0 +1,161 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perMinute + +class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand(), + ParallelCommandGroup( + WaitCommand(0.1).andThen(superstructure.groundIntakeCommand()), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.4.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.8.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + ), + superstructure.prepManualSpeakerCommand( + -3.7.degrees, 3000.rotations.perMinute, 0.7.degrees + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(2.8.meters, 5.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((2.84.meters + 8.3.meters) / 2, 6.45.meters).translation2d, + null, + 170.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(8.3.meters, 5.75.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), // Second leftmost centerline note + FieldWaypoint( + Translation2d((2.84.meters + 8.3.meters) / 2, 6.55.meters).translation2d, + null, + 170.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.84.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + superstructure + .scoreCommand() + .withTimeout(0.5) + .andThen(WaitCommand(0.75)) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(0.25)) + .andThen( + superstructure.prepManualSpeakerCommand( + -3.7.degrees, 3000.rotations.perMinute, 0.7.degrees + ) + ) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(2.84.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.3.meters, 6.4.meters).translation2d, + null, + 200.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.8.meters, (6.85 + 0.1).meters).translation2d, + null, + 207.89.degrees.inRotation2ds + ), + ) + } + ), + superstructure + .scoreCommand() + .andThen(superstructure.groundIntakeCommand()) + .andThen( + superstructure.prepManualSpeakerCommand( + -3.degrees, 3000.rotations.perMinute, 0.7.degrees + ) + ) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(2.8.meters, (6.85 + 0.1).meters).translation2d, + null, + 207.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.25.meters, 4.9.meters).translation2d, + null, + 158.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.75.meters, 4.15.meters).translation2d, + null, + 152.degrees.inRotation2ds + ), + ) + } + ), + superstructure + .scoreCommand() + .andThen(WaitCommand(0.1)) + .andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepManualSpeakerCommand(-3.degrees, 3000.rotations.perMinute, 0.7.degrees), + superstructure.scoreCommand().withTimeout(0.5) + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.4.meters, 5.50.meters), 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt index 639f7f6d..5a6772f5 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -21,8 +21,9 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru addRequirements(drivetrain, superstructure) addCommands( - superstructure.scoreCommand(), - WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.5), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -34,28 +35,31 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(2.9.meters - 0.75.meters, 6.9.meters).translation2d, + Translation2d(2.9.meters - 0.4.meters, 6.9.meters + 0.1.meters) + .translation2d, null, 180.degrees.inRotation2ds, ), FieldWaypoint( - Translation2d(2.9.meters, 6.9.meters).translation2d, + Translation2d(2.9.meters, 6.9.meters + 0.1.meters).translation2d, null, 180.degrees.inRotation2ds, ), FieldWaypoint( - Translation2d(1.435.meters, 5.535.meters).translation2d, + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ) ) - }, - keepTrapping = false - ), - WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.75).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepSpeakerLowCommand(), - superstructure.scoreCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.3), WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -63,33 +67,35 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru { listOf( FieldWaypoint( - Translation2d(1.435.meters, 5.535.meters).translation2d, + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ), // Subwoofer FieldWaypoint( - Translation2d(2.41.meters - 0.75.meters, 4.14.meters).translation2d, + Translation2d(2.41.meters - 0.4.meters, 4.1.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(2.41.meters + 0.225.meters, 4.14.meters).translation2d, + Translation2d(2.41.meters + 0.225.meters, 4.1.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(1.42.meters, 5.535.meters).translation2d, + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ) ) - }, - keepTrapping = false - ), + } + ) + .withTimeout(3.235 + 0.5), WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepSpeakerLowCommand(), - superstructure.scoreCommand(), + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.3), WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -97,13 +103,14 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru { listOf( FieldWaypoint( - Translation2d(1.42.meters, 5.535.meters).translation2d, + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( Translation2d( - ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + 0.25.meters, + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, 5.55.meters ) .translation2d, @@ -118,7 +125,8 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru ), FieldWaypoint( Translation2d( - ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + 0.25.meters, + ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 + + 0.25.meters, 5.45.meters ) .translation2d, @@ -126,18 +134,20 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(1.36.meters, 5.535.meters).translation2d, + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds ) // Subwoofer ) - }, - keepTrapping = false - ), - WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + } + ) + .withTimeout(3.235 + 0.5), + WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) ), superstructure.prepSpeakerLowCommand(), - superstructure.scoreCommand() + superstructure + .scoreCommand() + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.3) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt index 5c0f702d..5a7f35f9 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveCenterSubwooferAutoPath.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.auto.mode import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Superstructure import edu.wpi.first.wpilibj2.command.SequentialCommandGroup @@ -24,7 +25,7 @@ class PreloadAndLeaveCenterSubwooferAutoPath( addCommands( superstructure.scoreCommand(), - WaitCommand(secondaryWaitTime.inSeconds), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), DrivePathCommand.createPathInFieldFrame( drivetrain, { diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveLeftSubwooferAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveFromAmpSubwooferAutoPath.kt similarity index 87% rename from src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveLeftSubwooferAutoPath.kt rename to src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveFromAmpSubwooferAutoPath.kt index d395b717..0341b2a9 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveLeftSubwooferAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveFromAmpSubwooferAutoPath.kt @@ -14,7 +14,7 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds -class PreloadAndLeaveLeftSubwooferAutoPath( +class PreloadAndLeaveFromAmpSubwooferAutoPath( val drivetrain: Drivetrain, val superstructure: Superstructure, secondaryWaitTime: Time @@ -37,17 +37,17 @@ class PreloadAndLeaveLeftSubwooferAutoPath( FieldWaypoint( Translation2d(1.90.meters, 6.76.meters).translation2d, null, - -180.degrees.inRotation2ds + 180.degrees.inRotation2ds ), FieldWaypoint( Translation2d(2.87.meters, 6.27.meters).translation2d, null, - -180.degrees.inRotation2ds + 180.degrees.inRotation2ds ), FieldWaypoint( Translation2d(8.16.meters, 6.69.meters).translation2d, null, - -180.degrees.inRotation2ds + 180.degrees.inRotation2ds ) ) } @@ -56,6 +56,6 @@ class PreloadAndLeaveLeftSubwooferAutoPath( } companion object { - val startingPose = Pose2d(0.75.meters, 6.70.meters, (-120).degrees) + val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.degrees) } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveRightSubwooferAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveFromSourceSubwooferAutoPath.kt similarity index 97% rename from src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveRightSubwooferAutoPath.kt rename to src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveFromSourceSubwooferAutoPath.kt index f52545b2..ecd6dfab 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveRightSubwooferAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/PreloadAndLeaveFromSourceSubwooferAutoPath.kt @@ -14,7 +14,7 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds -class PreloadAndLeaveRightSubwooferAutoPath( +class PreloadAndLeaveFromSourceSubwooferAutoPath( val drivetrain: Drivetrain, val superstructure: Superstructure, secondaryWaitTime: Time diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteAutoPath.kt new file mode 100644 index 00000000..21ce2506 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteAutoPath.kt @@ -0,0 +1,179 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perSecond + +class SixNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.37.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.65.meters, 4.26.meters).translation2d, + null, + 152.degrees.inRotation2ds + ), + ) + }, + ), + WaitCommand(0.25).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepManualSpeakerCommand(-2.degrees), + superstructure.scoreCommand().withTimeout(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(2.65.meters, 4.26.meters).translation2d, + null, + 152.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.2.meters, 5.0.meters).translation2d, + null, + 210.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.84.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + }, + ), + WaitCommand(0.25).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepManualSpeakerCommand(-3.degrees), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(2.84.meters, 5.50.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.2.meters, 6.4.meters).translation2d, + null, + 200.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.75.meters, 6.85.meters).translation2d, + null, + 207.89.degrees.inRotation2ds + ), + ) + }, + ), + WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + ), + superstructure.prepManualSpeakerCommand(-2.degrees), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(2.75.meters, 6.85.meters).translation2d, + null, + 207.89.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.3.meters, 5.75.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.8.meters, 6.3.meters).translation2d, + null, + 185.degrees.inRotation2ds + ), + ) + }, + ), + WaitCommand(0.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(1.5)) + .andThen( + superstructure.prepManualSpeakerCommand(7.degrees, 4500.rotations.perSecond) + ), + ), + superstructure.scoreCommand().withTimeout(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(5.8.meters, 6.3.meters).translation2d, + null, + 185.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(6.2.meters, 6.2.meters).translation2d, + null, + 175.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.0.meters, 4.0.meters).translation2d, + null, + 155.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(6.15.meters, 6.2.meters).translation2d, + null, + 175.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.8.meters, 6.3.meters).translation2d, + null, + 185.degrees.inRotation2ds + ) + ) + }, + ), + WaitCommand(0.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(0.5)) + .andThen( + superstructure.prepManualSpeakerCommand(7.degrees, 4500.rotations.perSecond) + ), + ), + superstructure.scoreCommand().withTimeout(0.5) + ) + } + + companion object { + val startingPose = Pose2d(Translation2d(1.37.meters, 5.50.meters), 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt deleted file mode 100644 index 2da3ff8c..00000000 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt +++ /dev/null @@ -1,80 +0,0 @@ -package com.team4099.robot2023.auto.mode - -import com.team4099.lib.trajectory.FieldWaypoint -import com.team4099.robot2023.commands.drivetrain.DrivePathCommand -import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup -import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Translation2d -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inRotation2ds - -class SixNoteCenterlineAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { - init { - addRequirements(drivetrain) - - addCommands( - ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(1.51.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Subwoofer - FieldWaypoint( - Translation2d(2.41.meters, 4.14.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Rightmost wing note - FieldWaypoint( - Translation2d(2.41.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Middle wing note - FieldWaypoint( - Translation2d(2.41.meters, 7.03.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Leftmost wing note - FieldWaypoint( - Translation2d(7.79.meters, 7.42.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Leftmost center line note - FieldWaypoint( - Translation2d(2.39.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Subwoofer - FieldWaypoint( - Translation2d(5.82.meters, 6.5.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // In order to avoid stage - FieldWaypoint( - Translation2d(7.79.meters, 5.78.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Second leftmost wing note - FieldWaypoint( - Translation2d(5.82.meters, 6.5.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // In order to avoid stage - FieldWaypoint( - Translation2d(2.39.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ) // Subwoofer - ) - }, - resetPose = true - ) - ) - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt deleted file mode 100644 index 24bcb535..00000000 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt +++ /dev/null @@ -1,95 +0,0 @@ -package com.team4099.robot2023.auto.mode - -import com.team4099.lib.trajectory.FieldWaypoint -import com.team4099.robot2023.commands.drivetrain.DrivePathCommand -import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup -import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Translation2d -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inRotation2ds - -class SixNoteCenterlineWithPickupAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { - init { - addRequirements(drivetrain) - - addCommands( - ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(1.51.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Subwoofer - FieldWaypoint( - Translation2d(2.41.meters, 4.14.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Rightmost wing note - FieldWaypoint( - Translation2d(2.41.meters, 5.49.meters).translation2d, - null, - 235.degrees.inRotation2ds - ), // Middle wing note - FieldWaypoint( - Translation2d(2.41.meters, 7.03.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Leftmost wing note - FieldWaypoint( - Translation2d((2.41.meters + 7.79.meters) / 2, 7.21.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Leftmost wing note - FieldWaypoint( - Translation2d(7.79.meters, 7.21.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Leftmost center line note - FieldWaypoint( - Translation2d(2.39.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Subwoofer - FieldWaypoint( - Translation2d(5.82.meters, 6.5.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // In order to avoid stage - FieldWaypoint( - Translation2d(7.79.meters, 5.78.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // Second leftmost wing note - FieldWaypoint( - Translation2d(4.83.meters, 4.07.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), // In order to avoid stage - FieldWaypoint( - Translation2d(2.39.meters, 5.49.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(4.83.meters, 4.07.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(7.77.meters, 4.03.meters).translation2d, - null, - 180.degrees.inRotation2ds - ) - ) - }, - resetPose = true - ) - ) - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 479d917d..9f5e456b 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -3,14 +3,16 @@ package com.team4099.robot2023.auto.mode import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Translation2d -import org.team4099.lib.units.base.feet +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds -class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { +class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructure) : + SequentialCommandGroup() { init { addRequirements(drivetrain) @@ -25,28 +27,35 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() { startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(16.0.feet, 10.0.feet).translation2d, + Translation2d(startingPose.x + 2.meters, startingPose.y + 0.02.meters) + .translation2d, null, - 210.0.degrees.inRotation2ds + (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds ), FieldWaypoint( - Translation2d(13.0.feet, 11.0.feet).translation2d, + Translation2d(startingPose.x + 4.meters, startingPose.y).translation2d, null, - 180.degrees.inRotation2ds + (startingPose.rotation + 45.degrees).inRotation2ds ), FieldWaypoint( - Translation2d(10.0.feet, 10.0.feet).translation2d, + Translation2d(startingPose.x + 2.meters, startingPose.y - 0.02.meters) + .translation2d, null, - 180.degrees.inRotation2ds + (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds + ), + FieldWaypoint( + Translation2d(startingPose.x, startingPose.y).translation2d, + null, + (startingPose.rotation).inRotation2ds ) ) }, - resetPose = true + useLowerTolerance = false ) ) } companion object { - val startingPose = Pose2d(10.feet, 10.feet, 180.0.degrees) + val startingPose = Pose2d(Translation2d(1.42.meters, 5.535.meters), 180.degrees) } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt new file mode 100644 index 00000000..6beba4c7 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -0,0 +1,178 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perMinute + +class ThreeNoteAndPickupCenterlineSourceAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + 0.degrees.inRotation2ds, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d + + Translation2d(2.inches, -2.inches).translation2d, + 0.degrees.inRotation2ds, + (startingPose.rotation - 47.546.degrees).inRotation2ds + ) + ) + } + ), + superstructure.prepManualSpeakerCommand(-20.degrees, 3000.rotations.perMinute) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d + + Translation2d(2.inches, -2.inches).translation2d, + null, + (startingPose.rotation - 47.546.degrees).inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 0.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(2.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.29.meters, 0.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.37583640633171).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) + .andThen( + superstructure.prepManualSpeakerCommand(5.degrees, 4000.rotations.perMinute) + ) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (4.33.meters + 8.29.meters) / 2, + (2.44 + 0.78).meters / 2 - 0.1.meters + ) + .translation2d, + null, + ((180 - 43.3758 + 210) / 2 + 10).degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(8.29.meters, 2.44.meters).translation2d, + null, + 210.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (4.33.meters + 8.29.meters) / 2, + (2.44 + 0.78).meters / 2 + 0.1.meters + ) + .translation2d, + null, + ((180 - 43.3758 + 210) / 2 + 10).degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(0.5)) + .andThen( + superstructure.prepManualSpeakerCommand(5.degrees, 4000.rotations.perMinute) + ) + ), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.84.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.12.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) + ) + ) + } + + companion object { + val startingPose = Pose2d(1.40.meters, 4.09.meters, 180.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt new file mode 100644 index 00000000..18f5fc59 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -0,0 +1,203 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perMinute + +class ThreeNoteCenterlineFromAmpAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand(), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(1.90.meters, 6.76.meters).translation2d, + null, + 210.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.87.meters, 6.27.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 7.45.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(2.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters, 7.45.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) + .andThen( + superstructure + .prepManualSpeakerCommand( + 8.870702276919971.degrees, 4000.rotations.perMinute + ) + .withTimeout(1.0) + ) + ), + superstructure.scoreCommand().withTimeout(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 5.78.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters, 5.78.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) + .andThen( + superstructure.prepManualSpeakerCommand(2.degrees, 4000.rotations.perMinute) + ) + ), + superstructure.scoreCommand().withTimeout(0.5), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) + .andThen( + superstructure.prepManualSpeakerCommand(2.degrees, 4000.rotations.perMinute) + ) + ), + superstructure.scoreCommand() + ) + } + + companion object { + val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TwoNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TwoNoteCenterlineFromAmpAutoPath.kt new file mode 100644 index 00000000..96c4b30d --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TwoNoteCenterlineFromAmpAutoPath.kt @@ -0,0 +1,84 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class TwoNoteCenterlineFromAmpAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + // superstructure.prepSpeakerLowCommand(), + // superstructure.scoreCommand().withTimeout(0.5), + // WaitCommand(0.25), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(1.90.meters, 6.76.meters).translation2d, + null, + 210.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.87.meters, 6.27.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 7.45.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(2.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.27.meters, 7.45.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) + .andThen(superstructure.prepManualSpeakerCommand(8.870702276919971.degrees)) + ), + superstructure.scoreCommand() + ) + } + + companion object { + val startingPose = Pose2d(0.75.meters, 6.70.meters, 240.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TwoNoteCenterlineFromSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TwoNoteCenterlineFromSourceAutoPath.kt new file mode 100644 index 00000000..b1ce6480 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TwoNoteCenterlineFromSourceAutoPath.kt @@ -0,0 +1,86 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inRotation2ds + +class TwoNoteCenterlineFromSourceAutoPath( + val drivetrain: Drivetrain, + val superstructure: Superstructure +) : SequentialCommandGroup() { + init { + addRequirements(drivetrain) + addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.25), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.17.meters, 2.12.meters).translation2d, + null, + ((startingPose.rotation.inDegrees + 180) / 2).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.2.meters, 0.77.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ) + .withTimeout(3.828 + 0.25), + WaitCommand(2.0).andThen(superstructure.groundIntakeCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(8.2.meters, 0.77.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(2.17.meters, 2.12.meters).translation2d, + null, + ((startingPose.rotation.inDegrees + 180) / 2).degrees.inRotation2ds + ), + FieldWaypoint( + startingPose.translation.translation2d, + null, + startingPose.rotation.inRotation2ds + ) + ) + } + ), + WaitCommand(0.5).andThen(superstructure.prepSpeakerLowCommand()) + ), + superstructure.scoreCommand().withTimeout(0.5), + WaitCommand(0.25), + ) + } + + companion object { + val startingPose = Pose2d(0.63.meters, 4.44.meters, 120.degrees) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt index 0ba09b14..afefc58c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt @@ -1,8 +1,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DriveModuleSteeringCommand(val drivetrain: Drivetrain) : Command() { @@ -20,6 +20,6 @@ class DriveModuleSteeringCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - Logger.recordOutput("ActiveCommands/ZeroSensorsCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt index bd56d9ed..b73b6c7b 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem -import org.littletonrobotics.junction.Logger import java.util.function.BiConsumer import java.util.function.Consumer import java.util.function.Supplier @@ -108,7 +108,7 @@ class SysIdCommand : Command { data += (subsystemData.velRadPerSec / (2 * Math.PI)).toString() + "," } - Logger.recordOutput("ActiveCommands/SysIdCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/SysIdCommand", true) } // Called once the command ends or is interrupted. diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt new file mode 100644 index 00000000..ebd53be6 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/FeedForwardCharacterizationCommand.kt @@ -0,0 +1,99 @@ +package com.team4099.robot2023.commands.characterization + +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.utils.PolynomialRegression +import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inMetersPerSecond +import java.util.LinkedList +import java.util.function.Consumer +import java.util.function.Supplier + +class FeedForwardCharacterizationCommand( + val drivetrain: Drivetrain, + val voltageConsumer: Consumer, + val velocitySupplier: Supplier +) : Command() { + private val startDelay = 1.0.seconds + private val rampRatePerSecond = 0.1.volts + private val timer = Timer() + private lateinit var data: FeedForwardCharacterizationData + + init { + addRequirements(drivetrain) + } + + override fun initialize() { + data = FeedForwardCharacterizationData() + timer.reset() + timer.start() + } + + override fun execute() { + drivetrain.swerveModules.forEach { it.zeroSteering() } + + if (timer.get() < startDelay.inSeconds) { + voltageConsumer.accept(0.volts) + } else { + val voltage = ((timer.get() - startDelay.inSeconds) * rampRatePerSecond.inVolts).volts + voltageConsumer.accept(voltage) + data.add(velocitySupplier.get(), voltage) + Logger.recordOutput("Drivetrain/appliedVoltage", voltage.inVolts) + } + } + + override fun end(interrupted: Boolean) { + voltageConsumer.accept(0.volts) + timer.stop() + data.print() + } + + override fun isFinished(): Boolean { + return false + } + + companion object { + class FeedForwardCharacterizationData { + private val velocityData: MutableList = LinkedList() + private val voltageData: MutableList = LinkedList() + fun add(velocity: LinearVelocity, voltage: ElectricalPotential) { + if (velocity.absoluteValue.inMetersPerSecond > 1E-4) { + velocityData.add(velocity.absoluteValue) + voltageData.add(voltage.absoluteValue) + } + } + + fun print() { + if (velocityData.size == 0 || voltageData.size == 0) { + return + } + val regression = + PolynomialRegression( + velocityData + .stream() + .mapToDouble { obj: LinearVelocity -> obj.inMetersPerSecond } + .toArray(), + voltageData + .stream() + .mapToDouble { obj: ElectricalPotential -> obj.inVolts } + .toArray(), + 1 + ) + println("FF Characterization Results:") + println("\tCount=" + Integer.toString(velocityData.size) + "") + println(String.format("\tR2=%.5f", regression.R2())) + println(String.format("\tkS=%.5f", regression.beta(0))) + Logger.recordOutput("Drivetrain/kSFound", regression.beta(0)) + println(String.format("\tkV=%.5f", regression.beta(1))) + Logger.recordOutput("Drivetrain/kVFound", regression.beta(1)) + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt index 64a2d1b0..67f089a5 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt @@ -9,20 +9,20 @@ import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.math.MathUtil import edu.wpi.first.math.filter.SlewRateLimiter import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inInches import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.angle import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.radians -import kotlin.math.abs +import org.team4099.lib.units.derived.rotations import kotlin.math.hypot +import kotlin.time.times class WheelRadiusCharacterizationCommand( val drivetrain: Drivetrain, @@ -50,7 +50,10 @@ class WheelRadiusCharacterizationCommand( override fun initialize() { lastGyroYawRads = gyroYawSupplier() accumGyroYawRads = 0.0.radians - startWheelPositions = drivetrain.swerveModules.map { it.modulePosition.angle.angle } + startWheelPositions = + drivetrain.swerveModules.map { + (it.inputs.drivePosition / (DrivetrainConstants.WHEEL_DIAMETER * Math.PI)).rotations + } omegaLimiter.reset(0.0) } @@ -69,20 +72,28 @@ class WheelRadiusCharacterizationCommand( accumGyroYawRads += MathUtil.angleModulus((gyroYawSupplier() - lastGyroYawRads).inRadians).radians lastGyroYawRads = gyroYawSupplier() - var averageWheelPosition = 0.0 - val wheelPositions = drivetrain.swerveModules.map { it.modulePosition.angle.angle } + var averageWheelPositionDelta = 0.0.radians + val wheelPositions = + drivetrain.swerveModules.map { + (it.inputs.drivePosition / (DrivetrainConstants.WHEEL_DIAMETER * Math.PI)).rotations + } for (i in 0 until 4) { - averageWheelPosition += abs((wheelPositions[i] - startWheelPositions[i]).inRadians) + averageWheelPositionDelta += ((wheelPositions[i] - startWheelPositions[i])).absoluteValue } - averageWheelPosition /= 4.0 + averageWheelPositionDelta /= 4.0 currentEffectiveWheelRadius = - (accumGyroYawRads.inRadians * driveRadius / averageWheelPosition).meters - Logger.recordOutput("Drivetrain/RadiusCharacterization/DrivePosition", averageWheelPosition) - Logger.recordOutput( + (accumGyroYawRads * driveRadius / averageWheelPositionDelta).meters + DebugLogger.recordDebugOutput( + "Drivetrain/RadiusCharacterization/DrivePosition", averageWheelPositionDelta.inRadians + ) + DebugLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads.inRadians ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( + "Drivetrain/RadiusCharacterization/LastGyroYawRads", lastGyroYawRads.inRadians + ) + DebugLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/CurrentWheelRadiusInches", currentEffectiveWheelRadius.inInches ) @@ -90,12 +101,21 @@ class WheelRadiusCharacterizationCommand( override fun end(interrupted: Boolean) { if (accumGyroYawRads <= (Math.PI * 2.0).radians) { - println("Not enough data for characterization") + DebugLogger.recordDebugOutput( + "Drivetrain/radiansOffFromWheelRadius", + ((Math.PI * 2.0).radians - accumGyroYawRads).inRadians + ) } else { - println("Effective Wheel Radius: ${currentEffectiveWheelRadius.inInches} inches") + DebugLogger.recordDebugOutput( + "Drivetrain/effectiveWheelRadius", currentEffectiveWheelRadius.inInches + ) } } + override fun isFinished(): Boolean { + return false + } + companion object { enum class Direction(val value: Int) { CLOCKWISE(-1), diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 42efb8a7..8fcf16bf 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -1,7 +1,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.lib.logging.toDoubleArray import com.team4099.lib.math.asPose2d import com.team4099.lib.math.asTransform2d import com.team4099.lib.trajectory.CustomHolonomicDriveController @@ -12,6 +11,7 @@ import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil +import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.FrameType import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -24,6 +24,8 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Transform2d @@ -40,22 +42,21 @@ import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.cos import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds import org.team4099.lib.units.derived.inMetersPerSecondPerMeter import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRadiansPerSecondPerRadian +import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianPerSecond +import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianSeconds import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds import org.team4099.lib.units.derived.perMeter import org.team4099.lib.units.derived.perMeterSeconds +import org.team4099.lib.units.derived.perRadian +import org.team4099.lib.units.derived.perRadianPerSecond +import org.team4099.lib.units.derived.perRadianSeconds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.sin -import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.inRadiansPerSecond @@ -70,7 +71,7 @@ private constructor( val drivetrain: Drivetrain, private val waypoints: Supplier>, val resetPose: Boolean = false, - val keepTrapping: Boolean = false, + val useLowerTolerance: Boolean = false, val flipForAlliances: Boolean = true, val endPathOnceAtReference: Boolean = true, val leaveOutYAdjustment: Boolean = false, @@ -82,36 +83,35 @@ private constructor( private val xPID: PIDController> private val yPID: PIDController> - private val thetaPID: PIDController> + private val thetaPID: ProfiledPIDController> private val swerveDriveController: CustomHolonomicDriveController private var trajCurTime = 0.0.seconds private var trajStartTime = 0.0.seconds + private var changeStartTimeOnExecute = true + var trajectoryGenerator = CustomTrajectoryGenerator() val thetakP = LoggedTunableValue( "Pathfollow/thetakP", - DrivetrainConstants.PID.AUTO_THETA_PID_KP, - Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + Pair({ it.inRadiansPerSecondPerRadian }, { it.radians.perSecond.perRadian }) ) val thetakI = LoggedTunableValue( "Pathfollow/thetakI", - DrivetrainConstants.PID.AUTO_THETA_PID_KI, Pair( - { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + { it.inRadiansPerSecondPerRadianSeconds }, { it.radians.perSecond.perRadianSeconds } ) ) val thetakD = LoggedTunableValue( "Pathfollow/thetakD", - DrivetrainConstants.PID.AUTO_THETA_PID_KD, Pair( - { it.inDegreesPerSecondPerDegreePerSecond }, - { it.degrees.perSecond.perDegreePerSecond } + { it.inRadiansPerSecondPerRadianPerSecond }, + { it.radians.perSecond.perRadianPerSecond } ) ) @@ -182,8 +182,11 @@ private constructor( init { addRequirements(drivetrain) - - if (RobotBase.isSimulation()) { + if (RobotBase.isReal()) { + thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) + } else { thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) @@ -192,10 +195,11 @@ private constructor( xPID = PIDController(poskP.get(), poskI.get(), poskD.get()) yPID = PIDController(poskP.get(), poskI.get(), poskD.get()) thetaPID = - PIDController( + ProfiledPIDController( thetakP.get(), thetakI.get(), thetakD.get(), + TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) ) thetaPID.enableContinuousInput(-PI.radians, PI.radians) @@ -214,35 +218,35 @@ private constructor( xPID.wpiPidController, yPID.wpiPidController, thetaPID.wpiPidController ) - if (keepTrapping) { - swerveDriveController.setTolerance(Pose2d(3.inches, 3.inches, 2.5.degrees).pose2d) + if (useLowerTolerance) { + swerveDriveController.setTolerance(Pose2d(3.inches, 3.inches, 1.5.degrees).pose2d) } else { - swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 10.degrees).pose2d) + swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 6.degrees).pose2d) } - } - - override fun initialize() { - odoTField = drivetrain.odomTField - pathTransform = - Transform2d( - Translation2d(waypoints.get()[0].translation), - waypoints.get()[0].holonomicRotation?.radians?.radians ?: drivePoseSupplier().rotation - ) // trajectory generation! generate(waypoints.get()) + } + override fun initialize() { val trajectory = trajectoryGenerator.driveTrajectory if (trajectory.states.size <= 1) { return } + odoTField = drivetrain.odomTField + pathTransform = + Transform2d( + Translation2d(waypoints.get()[0].translation), + waypoints.get()[0].holonomicRotation?.radians?.radians ?: drivePoseSupplier().rotation + ) + // if (resetPose) { // drivetrain.odometryPose = AllianceFlipUtil.apply(Pose2d(trajectory.initialPose)) // } trajStartTime = Clock.fpgaTime + trajectory.states[0].timeSeconds.seconds - thetaPID.reset() + thetaPID.reset(0.degrees) xPID.reset() yPID.reset() } @@ -254,6 +258,11 @@ private constructor( return } + if (changeStartTimeOnExecute) { + trajStartTime = Clock.fpgaTime + trajectory.states[0].timeSeconds.seconds + changeStartTimeOnExecute = false + } + trajCurTime = Clock.fpgaTime - trajStartTime var desiredState = trajectory.sample(trajCurTime.inSeconds) @@ -274,24 +283,14 @@ private constructor( lastSampledPose = targetHolonomicPose when (stateFrame) { FrameType.FIELD -> { - Logger.recordOutput( - "Pathfollow/fieldTRobotTargetVisualized", - targetHolonomicPose.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/fieldTRobotTargetVisualized", targetHolonomicPose.pose2d) - Logger.recordOutput( - "Pathfollow/fieldTRobot", robotPoseInSelectedFrame.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/fieldTRobot", robotPoseInSelectedFrame.pose2d) } FrameType.ODOMETRY -> { - Logger.recordOutput( - "Pathfollow/odomTRobotTargetVisualized", - targetHolonomicPose.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/odomTRobotTargetVisualized", targetHolonomicPose.pose2d) - Logger.recordOutput( - "Pathfollow/odomTRobot", robotPoseInSelectedFrame.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/odomTRobot", robotPoseInSelectedFrame.pose2d) } } @@ -310,23 +309,16 @@ private constructor( odoTField.inverse().asPose2d().transformBy(robotPoseInSelectedFrame.asTransform2d()) lastSampledPose = odoTField.asPose2d().transformBy(targetHolonomicPose.asTransform2d()) - Logger.recordOutput( - "Pathfollow/fieldTRobotTargetVisualized", - targetHolonomicPose.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/fieldTRobotTargetVisualized", targetHolonomicPose.pose2d) - Logger.recordOutput( - "Pathfollow/fieldTRobot", robotPoseInSelectedFrame.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/fieldTRobot", robotPoseInSelectedFrame.pose2d) } } } // flip lastSampledPose = AllianceFlipUtil.apply(lastSampledPose) - Logger.recordOutput( - "Pathfollow/targetPoseInStateFrame", lastSampledPose.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("Pathfollow/targetPoseInStateFrame", lastSampledPose.pose2d) if (flipForAlliances) { desiredState = AllianceFlipUtil.apply(desiredState) @@ -360,6 +352,16 @@ private constructor( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) + /* + drivetrain.setOpenLoop( + nextDriveState.omegaRadiansPerSecond.radians.perSecond, + nextDriveState.vxMetersPerSecond.meters.perSecond to nextDriveState.vyMetersPerSecond.meters.perSecond, + ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB, + true + ) + + */ + drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( nextDriveState, @@ -386,18 +388,11 @@ private constructor( Logger.recordOutput( "Pathfollow/Desired Angle in Degrees", desiredState.poseMeters.rotation.degrees ) - Logger.recordOutput( - "Pathfollow/Desired Angular Velocity in Degrees", - desiredRotation.velocityRadiansPerSec.radians.perSecond.inDegreesPerSecond - ) - Logger.recordOutput( - "Pathfollow/trajectory", edu.wpi.first.math.trajectory.Trajectory.proto, trajectory - ) Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference()) Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds) - Logger.recordOutput("ActiveCommands/DrivePathCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true) if (thetakP.hasChanged()) thetaPID.proportionalGain = thetakP.get() if (thetakI.hasChanged()) thetaPID.integralGain = thetakI.get() @@ -427,7 +422,7 @@ private constructor( } override fun end(interrupted: Boolean) { - Logger.recordOutput("ActiveCommands/DrivePathCommand", false) + DebugLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { DriverStation.reportError(errorString, true) // Stop where we are if interrupted @@ -454,7 +449,7 @@ private constructor( drivetrain: Drivetrain, waypoints: Supplier>, resetPose: Boolean = false, - keepTrapping: Boolean = false, + useLowerTolerance: Boolean = false, flipForAlliances: Boolean = true, endPathOnceAtReference: Boolean = true, leaveOutYAdjustment: Boolean = false, @@ -465,7 +460,7 @@ private constructor( drivetrain, waypoints, resetPose, - keepTrapping, + useLowerTolerance, flipForAlliances, endPathOnceAtReference, leaveOutYAdjustment, @@ -478,7 +473,7 @@ private constructor( drivetrain: Drivetrain, waypoints: Supplier>, resetPose: Boolean = false, - keepTrapping: Boolean = false, + useLowerTolerance: Boolean = false, flipForAlliances: Boolean = true, endPathOnceAtReference: Boolean = true, leaveOutYAdjustment: Boolean = false, @@ -489,7 +484,7 @@ private constructor( drivetrain, waypoints, resetPose, - keepTrapping, + useLowerTolerance, flipForAlliances, endPathOnceAtReference, leaveOutYAdjustment, diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt index 1c84301e..d3545ff7 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt @@ -1,8 +1,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -17,7 +17,7 @@ class ResetGyroYawCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.d } override fun execute() { - Logger.recordOutput("ActiveCommands/ResetGyroYawCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/ResetGyroYawCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index d305748e..b8e35831 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -1,10 +1,9 @@ package com.team4099.robot2023.commands.drivetrain -import com.team4099.lib.logging.toDoubleArray import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() { @@ -15,14 +14,12 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() override fun initialize() { drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose)) drivetrain.zeroGyroYaw(AllianceFlipUtil.apply(pose).rotation) - Logger.recordOutput( - "Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).toDoubleArray().toDoubleArray() - ) - Logger.recordOutput("ActiveCommands/ResetPoseCommand", true) + DebugLogger.recordDebugOutput("Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).pose2d) + DebugLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", true) } override fun isFinished(): Boolean { - Logger.recordOutput("ActiveCommands/ResetPoseCommand", false) + DebugLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", false) return true } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt index 9e3cd36d..8d2ddce4 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt @@ -1,8 +1,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { init { @@ -14,6 +14,6 @@ class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - Logger.recordOutput("ActiveCommands/ResetZeroCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt index fb723c9a..5ac1cd55 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt @@ -4,6 +4,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command @@ -32,8 +33,9 @@ class TargetAngleCommand( val turn: () -> Double, val slowMode: () -> Boolean, val drivetrain: Drivetrain, - val targetAngle: Angle + val targetAngle: () -> Angle ) : Command() { + private var thetaPID: PIDController> val thetakP = LoggedTunableValue( @@ -107,14 +109,14 @@ class TargetAngleCommand( override fun execute() { drivetrain.defaultCommand.end(true) - Logger.recordOutput("ActiveCommands/TargetAngleCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", true) Logger.recordOutput( "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees ) - val thetaFeedback = thetaPID.calculate(drivetrain.odomTRobot.rotation, targetAngle) - Logger.recordOutput("Testing/error", thetaPID.error.inDegrees) - Logger.recordOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) + val thetaFeedback = thetaPID.calculate(drivetrain.odomTRobot.rotation, targetAngle()) + DebugLogger.recordDebugOutput("Testing/error", thetaPID.error.inDegrees) + DebugLogger.recordDebugOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( @@ -129,7 +131,7 @@ class TargetAngleCommand( } override fun end(interrupted: Boolean) { - Logger.recordOutput("ActiveCommands/TargetAngleCommand", false) + DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( driver.rotationSpeedClampedSupplier(turn, slowMode), diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt new file mode 100644 index 00000000..4dadbafc --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -0,0 +1,136 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.limelight.LimelightVision +import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.driver.DriverProfile +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.math.PI + +class TargetNoteCommand( + val driver: DriverProfile, + val driveX: () -> Double, + val driveY: () -> Double, + val turn: () -> Double, + val slowMode: () -> Boolean, + val drivetrain: Drivetrain, + val limelight: LimelightVision +) : Command() { + + private var thetaPID: PIDController> + val thetakP = + LoggedTunableValue( + "NoteAlignment/noteThetakP", + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "NoteAlignment/noteThetakI", + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "NoteAlignment/noteThetakD", + Pair( + { it.inDegreesPerSecondPerDegreePerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + init { + addRequirements(drivetrain) + + thetaPID = + PIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + ) + + if (!(RobotBase.isSimulation())) { + + thetakP.initDefault(DrivetrainConstants.PID.LIMELIGHT_THETA_KP) + thetakI.initDefault(DrivetrainConstants.PID.LIMELIGHT_THETA_KI) + thetakD.initDefault(DrivetrainConstants.PID.LIMELIGHT_THETA_KD) + + thetaPID = + PIDController( + DrivetrainConstants.PID.LIMELIGHT_THETA_KP, + DrivetrainConstants.PID.LIMELIGHT_THETA_KI, + DrivetrainConstants.PID.LIMELIGHT_THETA_KD + ) + } else { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + + thetaPID = + PIDController( + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP, + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI, + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD + ) + } + + thetaPID.enableContinuousInput(-PI.radians, PI.radians) + } + + override fun initialize() { + thetaPID.reset() + + if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { + thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) + } + } + + override fun execute() { + + drivetrain.defaultCommand.end(true) + DebugLogger.recordDebugOutput("ActiveCommands/TargetNoteCommand", true) + + val thetaFeedback = thetaPID.calculate(limelight.targetGamePieceTx ?: 0.0.degrees, 0.0.degrees) + DebugLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) + DebugLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) + + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = false + ) + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + driver.rotationSpeedClampedSupplier(turn, slowMode), + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = true + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt new file mode 100644 index 00000000..7c125f42 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt @@ -0,0 +1,181 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.lib.logging.LoggedTunableNumber +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.subsystems.vision.Vision +import com.team4099.robot2023.util.driver.DriverProfile +import com.team4099.robot2023.util.inverse +import edu.wpi.first.math.filter.MedianFilter +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.math.PI +import kotlin.math.absoluteValue +import kotlin.math.atan2 + +class TargetSpeakerCommand( + val driver: DriverProfile, + val driveX: () -> Double, + val driveY: () -> Double, + val turn: () -> Double, + val slowMode: () -> Boolean, + val drivetrain: Drivetrain, + val vision: Vision +) : Command() { + private var thetaPID: PIDController> + val thetakP = + LoggedTunableValue( + "Pathfollow/thetaAmpkP", + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetaAmpkI", + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + Pair( + { it.inDegreesPerSecondPerDegreePerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + var desiredAngle = 0.0.degrees + private val sizeOfMedianFilter = + LoggedTunableNumber("TargetSpeakerCommand/sizeOfMedianFilter", 10.0) + var angleMedianFilter = MedianFilter(10) + + init { + addRequirements(drivetrain) + + thetaPID = + PIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + ) + + if (!(RobotBase.isSimulation())) { + + thetakP.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD) + + thetaPID = + PIDController( + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP, + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI, + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD + ) + } else { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + + thetaPID = + PIDController( + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP, + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI, + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD + ) + } + + thetaPID.enableContinuousInput(-PI.radians, PI.radians) + } + + override fun initialize() { + thetaPID.reset() // maybe do first for x? + angleMedianFilter.reset() + /* + if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { + thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) + } + + */ + } + + override fun execute() { + if (sizeOfMedianFilter.hasChanged()) { + angleMedianFilter = MedianFilter(sizeOfMedianFilter.get().toInt()) + } + + drivetrain.defaultCommand.end(true) + Logger.recordOutput("ActiveCommands/TargetAngleCommand", true) + Logger.recordOutput( + "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees + ) + + val odomTRobot = drivetrain.odomTRobot + val odomTSpeaker = drivetrain.odomTSpeaker + val robotTSpeaker = odomTRobot.inverse().transformBy(odomTSpeaker) + + desiredAngle = + atan2( + robotTSpeaker.y.inMeters - + ( + drivetrain.robotVelocity.y * + robotTSpeaker.translation.magnitude.absoluteValue / 5 + ) + .value, + robotTSpeaker.x.inMeters - + ( + drivetrain.robotVelocity.x * + robotTSpeaker.translation.magnitude.absoluteValue / 5 + ) + .value + ) + .radians + val filteredDesiredAngle = angleMedianFilter.calculate(desiredAngle.inDegrees) + + val thetaFeedback = + thetaPID.calculate(odomTRobot.rotation, odomTRobot.rotation + filteredDesiredAngle.degrees) + + Logger.recordOutput("TargetSpeakerCommand/desiredAngleInDegrees", desiredAngle.inDegrees) + Logger.recordOutput("TargetSpeakerCommand/filteredDesiredAngleInDegrees", filteredDesiredAngle) + Logger.recordOutput("TargetSpeakerCommand/errorInDegrees", thetaPID.error.inDegrees) + Logger.recordOutput("TargetSpeakerCommand/thetaFeedbackInDPS", thetaFeedback.inDegreesPerSecond) + Logger.recordOutput("TargetSpeakerCommand/relativeToRobotPose", robotTSpeaker.pose2d) + + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = true + ) + } + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + Logger.recordOutput("ActiveCommands/TargetAngleCommand", false) + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + driver.rotationSpeedClampedSupplier(turn, slowMode), + driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + fieldOriented = true + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 6ff97742..0ce65a00 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class TeleopDriveCommand( @@ -26,8 +26,9 @@ class TeleopDriveCommand( if (DriverStation.isTeleop()) { val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) - Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", true) } } override fun isFinished(): Boolean { @@ -35,6 +36,6 @@ class TeleopDriveCommand( } override fun end(interrupted: Boolean) { - Logger.recordOutput("ActiveCommands/TeleopDriveCommand", false) + DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt index 06de519e..7ab857e6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt @@ -2,9 +2,9 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.inRotation2ds class TestDriveCommand(val drivetrain: Drivetrain) : Command() { @@ -16,7 +16,7 @@ class TestDriveCommand(val drivetrain: Drivetrain) : Command() { 4.0, 0.0, 0.0, drivetrain.odomTRobot.rotation.inRotation2ds ) ) - Logger.recordOutput("ActiveCommands/TestDriveCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/TestDriveCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index e8510fb3..4bdabc03 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,8 +1,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { @@ -20,6 +20,6 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - Logger.recordOutput("ActiveCommands/ZeroSensorsCommand", true) + DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 57485833..4ec9936f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.config import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.DrivetrainConstants import edu.wpi.first.wpilibj.GenericHID import edu.wpi.first.wpilibj2.command.button.Trigger import org.team4099.lib.joystick.XboxOneGamepad @@ -27,7 +28,7 @@ object ControlBoard { get() = -driver.leftYAxis val turn: Double - get() = driver.rightXAxis + get() = driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT val slowMode: Boolean get() = driver.leftShoulderButton @@ -52,6 +53,7 @@ object ControlBoard { val prepLow = Trigger { operator.xButton } val prepHighProtected = Trigger { operator.bButton } val prepHigh = Trigger { operator.yButton } + val passingShot = Trigger { operator.leftShoulderButton } val extendClimb = Trigger { operator.dPadUp } val retractClimb = Trigger { operator.dPadDown } @@ -63,8 +65,12 @@ object ControlBoard { val characterizeWrist = Trigger { driver.rightShoulderButton } - val climbAlignFar = Trigger { driver.yButton } - val climbAlignLeft = Trigger { driver.xButton } - val climbAlignRight = Trigger { driver.bButton } + val climbAlignFar = Trigger { driver.dPadUp } + val climbAlignLeft = Trigger { driver.dPadLeft } + val climbAlignRight = Trigger { driver.dPadRight } + + val targetSpeaker = Trigger { driver.xButton } // TODO: switch back to climbAlignLeft + val climbAutoAlign = Trigger { driver.bButton } + // week0 controls } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index e6d2737e..41089e90 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -55,7 +55,7 @@ object Constants { object Tuning { const val TUNING_MODE = false - const val DEBUGING_MODE = true + const val DEBUGING_MODE = false const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 @@ -142,8 +142,7 @@ object Constants { const val FEEDER_MOTOR_ID = 61 } - object Led { - const val LED_CANDLE_ID = 61 - const val LED_BLINKEN_ID = 1 + object LED { + const val LED_CANDLE_ID = 1 } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 72f6ae64..41ec3a85 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -26,16 +26,19 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.kilo import org.team4099.lib.units.perSecond +import kotlin.math.PI import kotlin.math.sqrt object DrivetrainConstants { - const val FOC_ENABLED = false + const val FOC_ENABLED = true const val MINIMIZE_SKEW = false + const val TELEOP_TURNING_SPEED_PERCENT = 0.75 + const val OMOMETRY_UPDATE_FREQUENCY = 250.0 const val WHEEL_COUNT = 4 - val WHEEL_DIAMETER = 3.827.inches + val WHEEL_DIAMETER = (2.083 * 2).inches val DRIVETRAIN_LENGTH = 22.750.inches val DRIVETRAIN_WIDTH = 22.750.inches @@ -58,8 +61,8 @@ object DrivetrainConstants { val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 3.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 2.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 4.0.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 3.25.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3 @@ -67,24 +70,24 @@ object DrivetrainConstants { const val DRIVE_SENSOR_CPR = 2048 const val STEERING_SENSOR_CPR = 2048 - const val DRIVE_SENSOR_GEAR_RATIO = (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0) + const val DRIVE_SENSOR_GEAR_RATIO = (16.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0) const val STEERING_SENSOR_GEAR_RATIO = 7.0 / 150.0 val ALLOWED_STEERING_ANGLE_ERROR = 1.degrees val STEERING_SUPPLY_CURRENT_LIMIT = 20.0.amps - val DRIVE_SUPPLY_CURRENT_LIMIT = 60.0.amps - val DRIVE_THRESHOLD_CURRENT_LIMIT = 60.0.amps + val DRIVE_SUPPLY_CURRENT_LIMIT = 70.0.amps + val DRIVE_THRESHOLD_CURRENT_LIMIT = 70.0.amps val DRIVE_TRIGGER_THRESHOLD_TIME = 0.1.seconds - val DRIVE_STATOR_CURRENT_LIMIT = 60.0.amps + val DRIVE_STATOR_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds - val FRONT_LEFT_MODULE_ZERO = 6.06.radians // good - val FRONT_RIGHT_MODULE_ZERO = 0.25.radians // good - val BACK_LEFT_MODULE_ZERO = 6.19.radians // good - val BACK_RIGHT_MODULE_ZERO = 4.12.radians // good + val FRONT_LEFT_MODULE_ZERO = 3.28.radians + PI.radians // good + val FRONT_RIGHT_MODULE_ZERO = 2.9.radians + PI.radians // good + val BACK_LEFT_MODULE_ZERO = 5.65.radians + PI.radians // good + val BACK_RIGHT_MODULE_ZERO = 3.48.radians + PI.radians // good val STEERING_COMPENSATION_VOLTAGE = 10.volts val DRIVE_COMPENSATION_VOLTAGE = 12.volts @@ -101,9 +104,9 @@ object DrivetrainConstants { val AUTO_POS_KP: ProportionalGain> get() { if (RobotBase.isReal()) { - return 0.23.meters.perSecond / 1.0.meters // todo:4 + return 3.3.meters.perSecond / 1.0.meters // todo:4 } else { - return 7.0.meters.perSecond / 1.0.meters + return 10.0.meters.perSecond / 1.0.meters } } val AUTO_POS_KI: IntegralGain> @@ -118,29 +121,33 @@ object DrivetrainConstants { val AUTO_POS_KD: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.14.meters.perSecond / (1.0.meters.perSecond)) + return (0.6.meters.perSecond / (1.0.meters.perSecond)) .metersPerSecondPerMetersPerSecond // todo: 0.25 } else { return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond } } - val AUTO_THETA_ALLOWED_ERROR = 3.degrees + val LIMELIGHT_THETA_KP = 0.0.degrees.perSecond / 1.degrees + val LIMELIGHT_THETA_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) + val LIMELIGHT_THETA_KD = + (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val AUTO_THETA_PID_KP = 0.1.degrees.perSecond / 1.degrees // 0.1 - val AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) + val AUTO_THETA_ALLOWED_ERROR = 3.degrees + val AUTO_THETA_PID_KP = (1.05.radians.perSecond / 1.radians) + val AUTO_THETA_PID_KI = (0.0.radians.perSecond / (1.radians * 1.seconds)) val AUTO_THETA_PID_KD = - (0.02.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val TELEOP_ALIGN_PID_KP = 4.degrees.perSecond / 1.degrees + val TELEOP_ALIGN_PID_KP = 3.6.degrees.perSecond / 1.degrees val TELEOP_ALIGN_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val TELEOP_ALIGN_PID_KD = - (0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val SIM_AUTO_THETA_PID_KP = 5.degrees.perSecond / 1.degrees + val SIM_AUTO_THETA_PID_KP = 10.degrees.perSecond / 1.degrees val SIM_AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val SIM_AUTO_THETA_PID_KD = - (1.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val MAX_AUTO_ANGULAR_VEL = 270.0.degrees.perSecond val MAX_AUTO_ANGULAR_ACCEL = 600.0.degrees.perSecond.perSecond @@ -151,14 +158,14 @@ object DrivetrainConstants { val STEERING_KFF = 0.0.volts / 1.0.radians.perSecond // 0.0375 - val DRIVE_KP = 0.0.volts / 1.meters.perSecond + val DRIVE_KP = 1.45.volts / 1.meters.perSecond val DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds) - val DRIVE_KD = 0.0.volts / 1.meters.perSecond.perSecond + val DRIVE_KD = 0.1.volts / 1.meters.perSecond.perSecond val DRIVE_KFF = 12.0.volts / 4.1675.meters.perSecond - val DRIVE_KS = 0.5.volts - val DRIVE_KV = 0.11.volts / 1.0.meters.perSecond + val DRIVE_KS = 0.177.volts + val DRIVE_KV = 0.12.volts / 1.0.meters.perSecond val DRIVE_KA = 0.0.volts / 1.0.meters.perSecond.perSecond // val DRIVE_KS = 0.23677.volts @@ -167,6 +174,7 @@ object DrivetrainConstants { val SIM_DRIVE_KS = 0.0.volts val SIM_DRIVE_KV = 2.7.volts / 1.0.meters.perSecond + val SIM_DRIVE_KA = 0.0.volts / 1.0.meters.perSecond.perSecond val SIM_DRIVE_KP = 1.5.volts / 1.meters.perSecond val SIM_DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index ae6dc21f..3c51361a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -16,15 +16,15 @@ import org.team4099.lib.units.perSecond object ElevatorConstants { // TODO: Change values later based on CAD - val REAL_KP = 0.725.volts / 1.inches + val REAL_KP = 8.0.volts / 1.inches val REAL_KI = 0.0.volts / (1.inches * 1.seconds) val REAL_KD = 0.0.volts / (1.inches.perSecond) val CARRIAGE_MASS = 30.892.pounds val ELEVATOR_MAX_RETRACTION = 0.0.inches - val ELEVATOR_MAX_EXTENSION = 17.0.inches - val ELEVATOR_CLIMB_EXTENSION = 16.0.inches + val ELEVATOR_MAX_EXTENSION = 16.0.inches + val ELEVATOR_CLIMB_EXTENSION = 15.5.inches val LEADER_INVERTED = false val FOLLOWER_INVERTED = true @@ -43,28 +43,31 @@ object ElevatorConstants { val ELEVATOR_KS = 0.0.volts val ELEVATOR_KG = 0.28.volts - val ELEVATOR_KV = 0.48.volts / 1.inches.perSecond + val ELEVATOR_KV = 1.0.volts / 1.inches.perSecond val ELEVATOR_KA = 0.075.volts / 1.inches.perSecond.perSecond val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts val ENABLE_ELEVATOR = true - val ELEVATOR_IDLE_HEIGHT = 0.25.inches - val ELEVATOR_SOFT_LIMIT_EXTENSION = 17.5.inches - val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.75.inches + val ELEVATOR_IDLE_HEIGHT = 0.0.inches + val ELEVATOR_SOFT_LIMIT_EXTENSION = 16.0.inches + val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches val ELEVATOR_SAFE_THRESHOLD = 5.0.inches - val ELEVATOR_TOLERANCE = 0.75.inches + val ELEVATOR_TOLERANCE = 0.5.inches - val MAX_VELOCITY = 1.meters.perSecond - val MAX_ACCELERATION = 2.0.meters.perSecond.perSecond + val MAX_VELOCITY = 2.meters.perSecond + val MAX_ACCELERATION = 4.0.meters.perSecond.perSecond - val SHOOT_SPEAKER_LOW_POSITION = 0.25.inches + val SHOOT_SPEAKER_LOW_POSITION = 0.0.inches val SHOOT_SPEAKER_MID_POSITION = 16.0.inches val SHOOT_SPEAKER_HIGH_POSITION = 16.0.inches - val SHOOT_AMP_POSITION = 17.0.inches // 16 + + // week 1 amp amgle + // val SHOOT_AMP_POSITION = 17.0.degrees + val SHOOT_AMP_POSITION = 15.0.inches val SOURCE_NOTE_OFFSET = 0.0.inches val ELEVATOR_THETA_POS = 0.0.degrees val HOMING_STATOR_CURRENT = 3.0.amps @@ -79,7 +82,7 @@ object ElevatorConstants { val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps + val LEADER_STATOR_CURRENT_LIMIT = 30.0.amps val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 0b128980..3d66af21 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -11,6 +11,7 @@ import org.team4099.lib.units.perSecond object FeederConstants { + val CLEAN_UP_TIME = 0.18.seconds val FEEDER_MOTOR_INVERTED = false val FEEDER_IDLE_VOLTAGE = 0.0.volts val VOLTAGE_COMPENSATION = 12.0.volts @@ -26,12 +27,12 @@ object FeederConstants { var WAIT_BEFORE_DETECT_VELOCITY_DROP = 0.5.seconds val IDLE_VOLTAGE = 0.volts - val INTAKE_NOTE_VOLTAGE = 1.volts - val AUTO_INTAKE_NOTE_VOLTAGE = 1.volts + val INTAKE_NOTE_VOLTAGE = 5.0.volts + val AUTO_INTAKE_NOTE_VOLTAGE = 1.5.volts val SHOOT_NOTE_VOLTAGE = 2.volts val OUTTAKE_NOTE_VOLTAGE = (-6).volts - val beamBreakFilterTime = 0.1.seconds + val beamBreakFilterTime = 0.01.seconds } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index b9937744..eec6511a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -11,6 +11,7 @@ import org.team4099.lib.geometry.Translation3d import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees // Copyright (c) 2024 FRC 6328 @@ -47,11 +48,130 @@ object FieldConstants { var wingX = 229.201.inches var podiumX = 126.75.inches var startingLineX = 74.111.inches + var subwooferX = 28.inches + val edgeOfBumperToCenter = 12.75.inches + 3.5.inches val fieldAprilTags: List = listOf( - AprilTag(4, Pose3d()), - AprilTag(3, Pose3d(Translation3d(0.meters, 0.5.meters, 0.meters), Rotation3d())) + AprilTag( + 0, + Pose3d( + Translation3d(1.meters, 57.25.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 1, + Pose3d( + Translation3d(1.meters, 57.25.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 2, + Pose3d( + Translation3d(1.meters, 57.25.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 3, + Pose3d( + Translation3d(652.755.inches, 218.416.inches - 22.25.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 4, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 5, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 6, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 7, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 8, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 9, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 10, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 11, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 12, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 13, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 14, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 15, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ), + AprilTag( + 16, + Pose3d( + Translation3d(652.755.inches, 218.416.inches, 57.125.inches), + Rotation3d(0.degrees, 0.degrees, 180.degrees) + ) + ) ) val tags = AprilTagFields.k2024Crescendo @@ -62,6 +182,14 @@ object FieldConstants { var noteThickness = 2.inches + val bottomRightSpeaker = Pose2d(0.0.inches, 238.815.inches, 0.degrees) + val bottomLeftSpeaker = Pose2d(0.0.inches, 197.765.inches, 0.degrees) + val topRightSpeaker = Pose2d(18.055.inches, 238.815.inches, 0.degrees) + val topLeftSpeaker = Pose2d(18.055.inches, 197.765.inches, 0.degrees) + + // Center of the speaker opening for the blue alliance + val centerSpeakerOpening = bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5.seconds) + /** Staging locations for each note */ object StagingLocations { var centerlineX = fieldLength / 2.0 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 71258530..be4589f1 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -41,7 +41,7 @@ object FlywheelConstants { val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps - val FLYWHEEL_TOLERANCE = 300.0.rotations.perMinute + val FLYWHEEL_TOLERANCE = 150.0.rotations.perMinute object PID { val REAL_KP: ProportionalGain, Volt> = 0.00.volts / 1.0.rotations.perMinute val REAL_KI: IntegralGain, Volt> = @@ -56,20 +56,22 @@ object FlywheelConstants { 0.0.volts / (1.0.rotations.perMinute.perSecond) val REAL_FLYWHEEL_KS = 0.0.volts - val REAL_FLYWHEEL_KV = 0.011.volts / 1.radians.perSecond - val REAL_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond + val REAL_FLYWHEEL_KV = 0.245.volts / 1.radians.perSecond + val REAL_FLYWHEEL_KA = 1.2.volts / 1.radians.perSecond.perSecond val SIM_FLYWHEEL_KS = 0.0.volts - val SIM_FLYWHEEL_KV = 0.011.volts / 1.radians.perSecond - val SIM_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond + val SIM_FLYWHEEL_KV = 0.1.volts / 1.radians.perSecond + val SIM_FLYWHEEL_KA = 0.1.volts / 1.radians.perSecond.perSecond } val IDLE_VELOCITY = 0.0.rotations.perMinute - val SPEAKER_VELOCITY = 3000.rotations.perMinute - val AMP_VELOCITY = 5_000.rotations.perMinute - val TRAP_VELOCITY = 5_000.rotations.perMinute + val SPEAKER_VELOCITY = 2500.rotations.perMinute + val PASSING_SHOT_VELOCITY = 2_000.rotations.perMinute - val AMP_SCORE_TIME = 0.5.seconds - val SPEAKER_SCORE_TIME = 0.4.seconds + val AMP_VELOCITY = 1_000.rotations.perMinute + val TRAP_VELOCITY = 3_000.rotations.perMinute + + val AMP_SCORE_TIME = 0.1.seconds + val SPEAKER_SCORE_TIME = 0.1.seconds val EJECT_VELOCITY = 3_000.rotations.perMinute } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index bcc28efa..0682b554 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -17,17 +17,17 @@ object IntakeConstants { val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees) // TODO: Change gear ratio according to robot - val ROLLER_CURRENT_LIMIT = 50.0.amps + val ROLLER_CURRENT_LIMIT = 80.0.amps const val ROLLER_MOTOR_INVERTED = true const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated const val CENTER_WHEEL_GEAR_RATIO = 34.0 / 14.0 // TODO: Update the idle roller voltage later - val IDLE_ROLLER_VOLTAGE = 1.0.volts + val IDLE_ROLLER_VOLTAGE = 0.0.volts val IDLE_CENTER_WHEEL_VOLTAGE = 0.0.volts val INTAKE_ROLLER_VOLTAGE = -12.volts - val INTAKE_CENTER_WHEEL_VOLTAGE = -9.volts + val INTAKE_CENTER_WHEEL_VOLTAGE = -12.volts val OUTTAKE_ROLLER_VOLTAGE = (10).volts val OUTTAKE_CENTER_WHEEL_VOLTAGE = (10).volts } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt new file mode 100644 index 00000000..4d135228 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -0,0 +1,32 @@ +package com.team4099.robot2023.config.constants + +import com.ctre.phoenix.led.Animation +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.volts + +object LEDConstants { + val INTAKE_CURRENT_THRESHOLD = 15.amps + val OUTAKE_CURRENT_THRESHOLD = 20.amps + val LED_COUNT = 50 + + val BATTERY_WARNING_THRESHOLD = 12.3.volts + + enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int) { + // Gold + NO_NOTE(null, 0, 0, 0), + NOTHING(null, 0, 0, 0), + RED(null, 255, 0, 0), + BLUE(null, 0, 0, 255), + + // Blue + + HAS_NOTE(null, 0, 0, 255), + + // Red + LOW_BATTERY(null, 255, 105, 0), + + // Green + + CAN_SHOOT(null, 0, 255, 0) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index 450de710..1b335621 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -9,7 +9,7 @@ object ShooterConstants { // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps - val WRIST_GEAR_RATIO = 0.0 + val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 0.0 val WRIST_VOLTAGE_COMPENSATION = 0.0.volts val WRIST_STATOR_CURRENT_LIMIT = 0.0.amps diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt new file mode 100644 index 00000000..4083cfa2 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt @@ -0,0 +1,75 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.perMinute + +object SuperstructureConstants { + val distanceFlywheelSpeedTableReal = + listOf( + Pair( + FieldConstants.subwooferX + FieldConstants.edgeOfBumperToCenter, + 3000.rotations.perMinute + ), + Pair(55.78.inches, 3000.rotations.perMinute), + Pair(60.0.inches, 3000.rotations.perMinute), + Pair(71.2.inches, 3000.rotations.perMinute), + Pair(83.37.inches, 3500.rotations.perMinute), + Pair(91.2.inches, 3500.rotations.perMinute), + Pair(103.6.inches, 3500.rotations.perMinute), + Pair(114.0.inches, 3500.rotations.perMinute), + Pair(124.0.inches, 3500.rotations.perMinute), + Pair(128.5.inches, 3500.rotations.perMinute), + Pair(134.inches, 3500.rotations.perMinute), + Pair(146.9.inches, 4000.rotations.perMinute), + Pair(159.0.inches, 4000.rotations.perMinute), + Pair(173.9.inches, 4500.rotations.perMinute), + Pair(183.9.inches, 4500.rotations.perMinute), + Pair(194.6.inches, 4500.rotations.perMinute) + ) + + val distanceWristAngleTableReal = + listOf( + Pair(FieldConstants.subwooferX + FieldConstants.edgeOfBumperToCenter, -34.5.degrees), + Pair(49.7.inches, -30.degrees), + Pair(60.0.inches, -22.degrees), + Pair(71.2.inches, -16.degrees), + Pair(83.37.inches, -9.degrees), + Pair(91.2.inches, -6.degrees), + Pair(103.6.inches, -1.degrees), + Pair(114.0.inches, -2.degrees), + Pair(124.0.inches, 4.degrees), + Pair(134.inches, 6.degrees), + Pair(146.9.inches, 8.75.degrees), + Pair(159.0.inches, 9.5.degrees), + Pair(173.9.inches, 12.25.degrees), + Pair(183.9.inches, 12.5.degrees), + Pair(194.6.inches, 13.5.degrees) + ) + + val distanceFlywheelSpeedTableSim = + listOf( + Pair(1.meters, 3000.rotations.perMinute), + Pair(2.meters, 3500.rotations.perMinute), + Pair(3.meters, 4000.rotations.perMinute), + Pair(4.meters, 4500.rotations.perMinute), + Pair(5.meters, 5000.rotations.perMinute), + Pair(6.meters, 5500.rotations.perMinute), + Pair(7.meters, 6000.rotations.perMinute), + Pair(500.inches, 8000.rotations.perMinute) + ) + + val distanceWristAngleTableSim = + listOf( + Pair(46.5.inches, -35.degrees), + Pair(75.inches, -22.degrees), + Pair(117.inches, -11.5.degrees), + Pair(149.inches, -6.5.degrees), + Pair(191.inches, -2.degrees), + Pair(253.inches, 1.5.degrees), + Pair(321.inches, 4.degrees), + Pair(500.inches, 10.degrees) + ) +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index d95146fa..2903f1aa 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -4,7 +4,10 @@ import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform3d import org.team4099.lib.geometry.Translation3d import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians +import kotlin.math.tan object VisionConstants { const val FRONT_CAMERA_NAME = "limelight" @@ -14,7 +17,9 @@ object VisionConstants { const val SIM_POSE_TOPIC_NAME = "Odometry/groundTruthPose" const val POSE_TOPIC_NAME = "Odometry/pose" - const val NUM_OF_CAMERAS = 1 + const val NUM_OF_CAMERAS = 2 + + val TRUSTED_CAMERA_ORDER = arrayOf(1, 0) // val CAMERA_TRANSFORMS = // listOf( @@ -39,25 +44,38 @@ object VisionConstants { val CAMERA_TRANSFORMS = listOf( Transform3d( - Translation3d(12.75.inches, 7.3125.inches, 28.75.inches), // 18.69 - Rotation3d(180.degrees, 0.degrees, 0.degrees) - ), - // Transform3d( - // Translation3d(-10.965.inches, -11.85.inches, 16.437.inches), - // Rotation3d(0.0.degrees, 0.0.degrees, 180.degrees) - // ), - Transform3d( - Translation3d(-6.560.inches, -13.575.inches, 16.25.inches), - Rotation3d(0.0.degrees, 0.0.degrees, -40.degrees) - ), // camera facing rightward + Translation3d(12.653.inches, -9.1.inches, 14.25.inches), // 18.69 + Rotation3d(-5.3.degrees, 30.degrees, -72.77.degrees) + ), // left Transform3d( - Translation3d(-6.560.inches, 13.575.inches, 16.25.inches), - Rotation3d(180.0.degrees, 0.0.degrees, 40.degrees) - ) // camera facing leftward + Translation3d(4.8.inches, 0.inches, 17.164.inches), // 18.69 + Rotation3d(0.degrees, 30.degrees, 0.degrees) + ), // front ) val CAMERA_NAMES = listOf("parakeet_1", "parakeet_2", "parakeet_3") + val robotTtag = + Transform3d( + Translation3d( + 102.6.inches + 25.75.inches, + 14.inches + 74.inches + 13.inches + 3.25.inches, + 19.25.inches + 1.meters - 3.25.inches + ), + Rotation3d(0.degrees, 0.degrees, 0.degrees) + ) + + object CAMERA_OV2387 { + val CAMERA_PX = 1600 + val CAMERA_PY = 1200 + + val HORIZONTAL_FOV = 80.degrees // i made these up lol + val VERTICAL_FOV = 64.25.degrees + + val vpw = 2.0 * tan(HORIZONTAL_FOV.inRadians / 2) + val vph = 2.0 * tan(VERTICAL_FOV.inRadians / 2) + } + object Limelight { val LIMELIGHT_NAME = "limelight-zapdos" val HORIZONTAL_FOV = 59.6.degrees diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 59524e6f..8a193c77 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -12,7 +12,6 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.kilo import org.team4099.lib.units.perSecond @@ -22,22 +21,34 @@ object WristConstants { // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps + val PUSH_DOWN_VOLTAGE = -0.5.volts + + val EJECT_ANGLE = -15.degrees val WRIST_AXIS_TO_NOTE_HOLD_POSITION = 14.5.inches val WRIST_AXIS_TO_NOTE_LAUNCH_POSITION = 10.inches val NOTE_ANGLE_SIM_OFFSET = -24.degrees + val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 1.06488 / 1.0 + val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO = + 5.0 / 1.0 * 4.0 / 1.0 * 3.0 / 1.0 * 46.0 / 42.0 * 90.0 / 33.0 * 1.0 / 1.06488 + val VOLTAGE_COMPENSATION = 12.0.volts - val ABSOLUTE_ENCODER_OFFSET = 0.degrees + val ABSOLUTE_ENCODER_OFFSET = + ( + 97.72227856659904.degrees - 35.degrees + 1.3.degrees - + 0.33.degrees - + 1.degrees - + 0.5.degrees + ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared - val WRIST_ENCODER_GEAR_RATIO = 0.0 + // val WRIST_GEAR_RATIO = 1.0 / 5.0 * 1.0 / 4.0 * 1.0 / 3.0 * 42.0 / 46.0 * 33.0 / 90.0 - val WRIST_GEAR_RATIO = 1.0 / 5.0 * 1.0 / 4.0 * 1.0 / 3.0 * 42.0 / 46.0 * 33.0 / 90.0 val WRIST_VOLTAGE_COMPENSATION = 12.0.volts - val WRIST_STATOR_CURRENT_LIMIT = 10.0.amps - val WRIST_SUPPLY_CURRENT_LIMIT = 10.amps + val WRIST_STATOR_CURRENT_LIMIT = 30.0.amps + val WRIST_SUPPLY_CURRENT_LIMIT = 30.amps val WRIST_THRESHOLD_CURRENT_LIMIT = 1.0.amps val WRIST_TRIGGER_THRESHOLD_TIME = 10.0.seconds @@ -47,15 +58,32 @@ object WristConstants { val WRIST_ZERO_SIM_OFFSET = 27.5.degrees val MAX_WRIST_VELOCITY = 300.degrees.perSecond - val MAX_WRIST_ACCELERATION = 600.degrees.perSecond.perSecond + val MAX_WRIST_ACCELERATION = 1000.degrees.perSecond.perSecond val HARDSTOP_OFFSET = 47.degrees object PID { - val REAL_KP: ProportionalGain = 0.5.volts / 1.0.degrees + + val ARBITRARY_FEEDFORWARD = 0.03.volts + + val REAL_KP: ProportionalGain = 0.45.volts / 1.0.degrees val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) - val REAL_KD: DerivativeGain = 0.0.volts / (1.0.rotations / 1.0.seconds) + val REAL_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond + + val FIRST_STAGE_POS_SWITCH_THRESHOLD = 3.0.degrees + val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 5.0.degrees.perSecond - val SIM_KP: ProportionalGain = 1.volts / 1.0.degrees + val FIRST_STAGE_KP: ProportionalGain = 0.35.volts / 1.0.degrees + val FIRST_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) + val FIRST_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond + + val SECOND_STAGE_POS_SWITCH_THRESHOLD = 1.0.degrees + val SECOND_STAGE_VEL_SWITCH_THRESHOLD = 5.0.degrees.perSecond + + val SECOND_STAGE_KP: ProportionalGain = 1.5.volts / 1.0.degrees + val SECOND_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) + val SECOND_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond + + val SIM_KP: ProportionalGain = 10.volts / 1.0.degrees val SIM_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val SIM_KD: DerivativeGain = 0.0175.volts / (1.0.degrees / 1.0.seconds) @@ -70,15 +98,19 @@ object WristConstants { val SIM_WRIST_KS = 0.15.volts } - val WRIST_TOLERANCE = 0.1.degrees + val WRIST_TOLERANCE = 0.3.degrees - val IDLE_ANGLE = (-35.0).degrees - val AMP_SCORE_ANGLE = -16.0.degrees - val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = -36.0.degrees - val SUBWOOFER_SPEAKER_SHOT_ANGLE_MID = -7.5.degrees + val IDLE_ANGLE = (-34).degrees + + val AMP_SCORE_ANGLE = -8.0.degrees + val FAST_AMP_ANGLE = 35.degrees + val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = -34.degrees + val SUBWOOFER_SPEAKER_SHOT_ANGLE_MID = 8.0.degrees val SUBWOOFER_SPEAKER_SHOT_ANGLE_HIGH = -2.degrees val CLIMB_ANGLE = 10.0.degrees - val TRAP_ANGLE = 35.0.degrees - val INTAKE_ANGLE = (-35.0).degrees - val IDLE_ANGLE_HAS_GAMEPEICE = -35.0.degrees + + val TRAP_ANGLE = 35.degrees + val INTAKE_ANGLE = (-34).degrees + val IDLE_ANGLE_HAS_GAMEPEICE = -34.degrees + val PASSING_SHOT_ANGLE = -20.degrees } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 84273c0a..80d8b905 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -2,9 +2,9 @@ package com.team4099.robot2023.subsystems.drivetrain.drive import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.lib.logging.toDoubleArray import com.team4099.lib.math.asPose2d import com.team4099.lib.math.asTransform2d +import com.team4099.lib.vision.TimestampedTrigVisionUpdate import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants @@ -46,6 +46,7 @@ import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inRotation2ds import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import java.util.concurrent.locks.Lock @@ -60,7 +61,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - private val gyroInputs = GyroIO.GyroIOInputs() + val gyroInputs = GyroIO.GyroIOInputs() private var gyroYawOffset = 0.0.radians @@ -89,10 +90,12 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) private set - private var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) private var omegaVelocity = 0.0.radians.perSecond + private var characterizationInput = 0.0.volts + var lastGyroYaw = { gyroInputs.gyroYaw } var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED @@ -129,6 +132,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB is DrivetrainRequest.ZeroSensors -> { isInAutonomous = value.isInAutonomous } + is DrivetrainRequest.Characterize -> { + characterizationInput = value.voltage + } else -> {} } field = value @@ -198,6 +204,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB val odomTField: Transform2d get() = fieldFrameEstimator.getLatestOdometryTField() + val odomTSpeaker: Transform2d + get() = fieldFrameEstimator.getLatestOdometryTSpeaker() + private var undriftedPose: Pose2d get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters) set(value) { @@ -269,6 +278,11 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB // updating odometry every loop cycle updateOdometry() + Logger.recordOutput( + "FieldFrameEstimator/odomTSpeaker", + fieldFrameEstimator.getLatestOdometryTSpeaker().transform2d + ) + Logger.recordOutput("Drivetrain/OdometryGyroRotationValue", odomTRobot.rotation.inDegrees) Logger.recordOutput( @@ -289,9 +303,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) ) - Logger.recordOutput( - "" + "FieldRelativePose/robotPose", fieldTRobot.toDoubleArray().toDoubleArray() - ) + Logger.recordOutput("" + "FieldRelativePose/robotPose", fieldTRobot.pose2d) Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) Logger.recordOutput("Drivetrain/setPointStates", *setPointStates.toTypedArray()) @@ -311,7 +323,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB .pose3d ) - Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.toDoubleArray()) + Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) Logger.recordOutput( "Odometry/targetPose", @@ -363,6 +375,12 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.recordOutput("Drivetrain/TargetChassisSpeeds", targetedChassisSpeeds) Logger.recordOutput("Drivetrain/TargetChassisAccels", targetedChassisAccels) + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CHARACTERIZE -> { + swerveModules.forEach { it.runCharacterization(characterizationInput) } + // Transitions nextState = fromRequestToState(currentRequest) } @@ -486,6 +504,87 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } } + fun setOpenLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = + edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0), + fieldOriented: Boolean = true + ) { + + Logger.recordOutput("Drivetrain/isFieldOriented", fieldOriented) + // flip the direction base don alliance color + val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 + val allianceFlippedDriveVector = + Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) + + Logger.recordOutput( + "Drivetrain/driveVectorFirst", allianceFlippedDriveVector.first.inMetersPerSecond + ) + Logger.recordOutput( + "Drivetrain/driveVectorSecond", allianceFlippedDriveVector.second.inMetersPerSecond + ) + + val swerveModuleStates: Array + var desiredChassisSpeeds: ChassisSpeeds + + // calculated chasis speeds, apply field oriented transformation + if (fieldOriented) { + desiredChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + odomTRobot.rotation + ) + } else { + desiredChassisSpeeds = + ChassisSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + ) + } + + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy + ), + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega + ) + + val twistToNextPose: Twist2d = velocityTransform.log() + + desiredChassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) + } + + // convert target chassis speeds to individual module setpoint states + swerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) + val accelSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisAccels) + + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) + setPointStates = swerveModuleStates.toMutableList() + + // set each module openloop based on corresponding states + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + swerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) + } + } + /** * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular * and linear acceleration. Calculates both angular and linear velocities and acceleration and @@ -646,6 +745,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fieldFrameEstimator.addVisionData(visionData) } + fun addSpeakerVisionData(visionData: TimestampedTrigVisionUpdate) { + fieldFrameEstimator.addSpeakerVisionData(visionData) + } + fun lockWheels() { DrivetrainIOReal.getSwerveModules()[1].setOpenLoop( DrivetrainConstants.FL_LOCKING_ANGLE, 0.meters.perSecond, true @@ -683,7 +786,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ZEROING_SENSORS, OPEN_LOOP, LOCK_WHEELS, - CLOSED_LOOP; + CLOSED_LOOP, + CHARACTERIZE; inline fun equivalentToRequest(request: Request.DrivetrainRequest): Boolean { return ( @@ -691,7 +795,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || (request is DrivetrainRequest.Idle && this == IDLE) || - (request is DrivetrainRequest.LockWheels && this == LOCK_WHEELS) + (request is DrivetrainRequest.LockWheels && this == LOCK_WHEELS) || + (request is DrivetrainRequest.Characterize && this == CHARACTERIZE) ) } } @@ -703,6 +808,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS is DrivetrainRequest.Idle -> DrivetrainState.IDLE is DrivetrainRequest.LockWheels -> DrivetrainState.LOCK_WHEELS + is DrivetrainRequest.Characterize -> DrivetrainState.CHARACTERIZE } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index 98dfec22..d60a078a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -13,6 +13,7 @@ import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -54,6 +55,19 @@ class SwerveModule(val io: SwerveModuleIO) { private var shouldInvert = false + private val driveKV = + LoggedTunableValue( + "Drivetrain/kV", + DrivetrainConstants.PID.DRIVE_KV, + Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) + ) + private val driveKA = + LoggedTunableValue( + "Drivetrain/kA", + DrivetrainConstants.PID.DRIVE_KA, + Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) + ) + private val steeringkP = LoggedTunableValue( "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) @@ -114,6 +128,9 @@ class SwerveModule(val io: SwerveModuleIO) { drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) } + + driveKV.initDefault(DrivetrainConstants.PID.DRIVE_KV) + driveKA.initDefault(DrivetrainConstants.PID.DRIVE_KA) } fun updateInputs() { @@ -161,8 +178,15 @@ class SwerveModule(val io: SwerveModuleIO) { io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) } - if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { - io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) + if (drivekP.hasChanged() || + drivekI.hasChanged() || + drivekD.hasChanged() || + driveKV.hasChanged() || + driveKA.hasChanged() + ) { + io.configureDrivePID( + drivekP.get(), drivekI.get(), drivekD.get(), driveKV.get(), driveKA.get() + ) } Logger.processInputs(io.label, inputs) @@ -352,4 +376,8 @@ class SwerveModule(val io: SwerveModuleIO) { fun setSteeringBrakeMode(brake: Boolean) { io.setSteeringBrakeMode(brake) } + + fun runCharacterization(input: ElectricalPotential) { + io.runCharacterization(input) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt index 3ac6798e..b7cfcbb2 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt @@ -4,8 +4,10 @@ import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.Fraction import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter @@ -17,6 +19,7 @@ import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian @@ -135,6 +138,7 @@ interface SwerveModuleIO { fun setSteeringSetpoint(angle: Angle) {} fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} + fun runCharacterization(input: ElectricalPotential) {} fun resetModuleZero() {} fun zeroSteering() {} @@ -147,7 +151,9 @@ interface SwerveModuleIO { fun configureDrivePID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> + kD: DerivativeGain, Volt>, + kV: Value>>, + kA: Value>>> ) {} fun configureSteeringPID( kP: ProportionalGain, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index 07bd876a..19ac3d21 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt @@ -6,6 +6,7 @@ import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor +import edu.wpi.first.math.MathUtil import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.wpilibj.simulation.BatterySim import edu.wpi.first.wpilibj.simulation.FlywheelSim @@ -15,8 +16,10 @@ import org.team4099.lib.controller.PIDController import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.Fraction import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter @@ -76,7 +79,9 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { ) private val driveFeedForward = SimpleMotorFeedforward( - DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV + DrivetrainConstants.PID.SIM_DRIVE_KS, + DrivetrainConstants.PID.SIM_DRIVE_KV, + DrivetrainConstants.PID.SIM_DRIVE_KA ) private val steeringFeedback = @@ -252,7 +257,9 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { override fun configureDrivePID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> + kD: DerivativeGain, Volt>, + kV: Value>>, + kA: Value>>> ) { driveFeedback.setPID(kP, kI, kD) } @@ -275,4 +282,9 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { ) { println("Can't configure motion magic in simulation") } + + override fun runCharacterization(input: ElectricalPotential) { + val appliedVolts = MathUtil.clamp(input.inVolts, -12.0, 12.0) + driveMotorSim.setInputVoltage(appliedVolts) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index b810c02b..320d5602 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -12,7 +12,6 @@ import com.ctre.phoenix6.controls.VelocityVoltage import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue -import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 @@ -24,8 +23,10 @@ import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.Fraction import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps @@ -37,14 +38,16 @@ import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.ctreLinearMechanismSensor import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond -import org.team4099.lib.units.derived.perMeterPerSecond +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecondPerSecond import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond @@ -77,20 +80,13 @@ class SwerveModuleIOTalon( private val potentiometerOutput: Double get() { - return if (label == Constants.Drivetrain.FRONT_LEFT_MODULE_NAME) { + return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) { potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * PI } else { 2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI } } - private val driveKV = - LoggedTunableValue( - "Drivetrain/kV", - DrivetrainConstants.PID.DRIVE_KV, - Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) - ) - val driveStatorCurrentSignal: StatusSignal val driveSupplyCurrentSignal: StatusSignal val steeringStatorCurrentSignal: StatusSignal @@ -140,7 +136,9 @@ class SwerveModuleIOTalon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = driveKV.get().inVoltsPerMetersPerSecond + driveConfiguration.Slot0.kV = DrivetrainConstants.PID.DRIVE_KV.inVoltsPerMetersPerSecond + driveConfiguration.Slot0.kA = + DrivetrainConstants.PID.DRIVE_KA.inVoltsPerMetersPerSecondPerSecond // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes @@ -279,6 +277,7 @@ class SwerveModuleIOTalon( } override fun setSteeringSetpoint(angle: Angle) { + Logger.recordOutput("$label/steeringSetpointDegrees", angle.inDegrees) steeringFalcon.setControl( PositionDutyCycle( steeringSensor.positionToRawUnits(angle), @@ -316,7 +315,7 @@ class SwerveModuleIOTalon( } /** - * Open Loop Control using PercentO`utput control on a Falcon + * Open Loop Control using PercentOutput control on a Falcon * * @param steering: Desired angle * @param speed: Desired speed @@ -341,7 +340,7 @@ class SwerveModuleIOTalon( override fun zeroSteering() { steeringFalcon.setPosition( steeringSensor.positionToRawUnits( - if (label != Constants.Drivetrain.FRONT_LEFT_MODULE_NAME) + if (label != Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) else (potentiometerOutput.radians - zeroOffset) ) @@ -362,14 +361,17 @@ class SwerveModuleIOTalon( override fun configureDrivePID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> + kD: DerivativeGain, Volt>, + kV: Value>>, + kA: Value>>> ) { val PIDConfig = Slot0Configs() PIDConfig.kP = driveSensor.proportionalVelocityGainToRawUnits(kP) PIDConfig.kI = driveSensor.integralVelocityGainToRawUnits(kI) PIDConfig.kD = driveSensor.derivativeVelocityGainToRawUnits(kD) - PIDConfig.kV = driveKV.get().inVoltsPerMetersPerSecond + PIDConfig.kV = kV.inVoltsPerMetersPerSecond + PIDConfig.kA = kA.inVoltsPerMetersPerSecondPerSecond driveFalcon.configurator.apply(PIDConfig) } @@ -426,4 +428,12 @@ class SwerveModuleIOTalon( // motor output configs might overwrite invert? steeringFalcon.inverted = true } + + override fun runCharacterization(input: ElectricalPotential) { + if (label == Constants.Drivetrain.FRONT_LEFT_MODULE_NAME) { + driveFalcon.setControl(DutyCycleOut(-input.inVolts / 12.0)) + } else { + driveFalcon.setControl(DutyCycleOut(input.inVolts / 12.0)) + } + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 16e04328..13b46153 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -54,7 +54,7 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() { private val kG = LoggedTunableValue("Elevator/kG", Pair({ it.inVolts }, { it.volts })) private val kV = LoggedTunableValue( - "Elevator/kG", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) + "Elevator/kV", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) ) private val kA = LoggedTunableValue( @@ -238,6 +238,11 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() { io.configPID(kP.get(), kI.get(), kD.get()) } + kV.initDefault(ElevatorConstants.ELEVATOR_KV) + kS.initDefault(ElevatorConstants.ELEVATOR_KS) + kG.initDefault(ElevatorConstants.ELEVATOR_KG) + kA.initDefault(ElevatorConstants.ELEVATOR_KA) + elevatorFeedforward = ElevatorFeedforward( ElevatorConstants.ELEVATOR_KS, @@ -252,6 +257,10 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() { if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { io.configPID(kP.get(), kI.get(), kD.get()) } + + if (kS.hasChanged() || kV.hasChanged() || kA.hasChanged() || kG.hasChanged()) { + elevatorFeedforward = ElevatorFeedforward(kS.get(), kG.get(), kV.get(), kA.get()) + } Logger.processInputs("Elevator", inputs) Logger.recordOutput("Elevator/currentState", currentState.name) Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 4118a3ea..2eb2cff3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -134,8 +134,8 @@ object ElevatorIOKraken : ElevatorIO { pidConfiguration.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) pidConfiguration.kI = leaderSensor.integralPositionGainToRawUnits(kI) pidConfiguration.kD = leaderSensor.derivativePositionGainToRawUnits(kD) + elevatorLeaderKraken.configurator.apply(pidConfiguration) - elevatorFollowerKraken.configurator.apply(pidConfiguration) } override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index 7201dffa..93add24d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -94,8 +94,8 @@ object ElevatorIONEO : ElevatorIO { ), ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, 30.celsius, - ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, - 90.celsius + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 10.amps, + 80.celsius ), ) } @@ -189,8 +189,5 @@ object ElevatorIONEO : ElevatorIO { leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) - followerPIDController.p = followerSensor.proportionalPositionGainToRawUnits(kP) - followerPIDController.i = followerSensor.integralPositionGainToRawUnits(kI) - followerPIDController.d = followerSensor.derivativePositionGainToRawUnits(kD) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt index d8481e41..2f88041b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.falconspin +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj.DriverStation import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inAmperes @@ -26,7 +27,9 @@ object MotorChecker { } fun periodic() { - Logger.recordOutput("MotorChecker/subsystemNames", subsystemHardware.keys.toTypedArray()) + DebugLogger.recordDebugOutput( + "MotorChecker/subsystemNames", subsystemHardware.keys.toTypedArray() + ) for (subsystemName in subsystemHardware.keys) { @@ -66,7 +69,9 @@ object MotorChecker { } } } - Logger.recordOutput("MotorChecker/$subsystemName/motorNames", motorNames.toTypedArray()) + DebugLogger.recordDebugOutput( + "MotorChecker/$subsystemName/motorNames", motorNames.toTypedArray() + ) Logger.recordOutput( "MotorChecker/baseStageTriggered", baseStageCurrentLimitTriggered.toTypedArray() @@ -84,53 +89,57 @@ object MotorChecker { // not clean but whatever fun logMotor(subsystemName: String, motor: Motor) { - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/AppliedVoltageVolts", motor.appliedVoltage.inVolts ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/BusVoltageVolts", motor.busVoltage.inVolts ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/TemperatureCelsius", motor.temperature.inCelsius ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/StatorCurrentAmps", motor.statorCurrent.inAmperes ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/SupplyCurrentAmps", motor.supplyCurrent.inAmperes ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/CurrentLimitStage", motor.currentLimitStage.name ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/BaseCurrentLimitAmps", motor.baseCurrentLimit.inAmperes ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/FirstStageTemperatureLimitCelsius", motor.firstStageTemperatureLimit.inCelsius ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/FirstStageCurrentLimitAmps", motor.firstStageCurrentLimit.inAmperes ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/MotorShutDownThresholdCelsius", motor.motorShutDownThreshold.inCelsius ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/CurrentLimitInUseAmps", motor.currentLimitInUse.inAmperes ) - Logger.recordOutput("MotorChecker/$subsystemName/${motor.name}/MotorID", motor.id.toLong()) - Logger.recordOutput( + DebugLogger.recordDebugOutput( + "MotorChecker/$subsystemName/${motor.name}/MotorID", motor.id.toLong() + ) + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Errors", motor.errors.toTypedArray() ) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Warnings", motor.warnings.toTypedArray() ) - Logger.recordOutput("MotorChecker/$subsystemName/${motor.name}/Info", motor.info.toTypedArray()) - Logger.recordOutput( + DebugLogger.recordDebugOutput( + "MotorChecker/$subsystemName/${motor.name}/Info", motor.info.toTypedArray() + ) + DebugLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/StickyFaults", motor.stickyFaults.toTypedArray() ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 603cf51d..de7268fb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -5,6 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.math.filter.Debouncer import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -145,7 +146,9 @@ class Feeder(val io: FeederIO) : SubsystemBase() { Logger.recordOutput( "Feeder/isAtCommandedState", currentState.equivalentToRequest(currentRequest) ) - Logger.recordOutput("Feeder/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) + DebugLogger.recordDebugOutput( + "Feeder/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds + ) Logger.recordOutput("Feeder/feederVoltageTarget", feederTargetVoltage.inVolts) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt index 8ba29e88..05dad96b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt @@ -45,7 +45,7 @@ object FeederIONeo : FeederIO { feederSparkMax.burnFlash() MotorChecker.add( - "Ground Intake", + "Feeder", "Roller", MotorCollection( mutableListOf(Neo(feederSparkMax, "Roller Motor")), diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index 800d8d95..1b26b753 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -5,6 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -82,6 +83,12 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { FlywheelConstants.AMP_VELOCITY, Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) ) + val passingShotVelocity = + LoggedTunableValue( + "Flywheel/passingShotVelocity", + FlywheelConstants.PASSING_SHOT_VELOCITY, + Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) + ) } private val kP = @@ -138,7 +145,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { currentState == FlywheelStates.TARGETING_VELOCITY && (inputs.leftFlywheelVelocity - flywheelLeftTargetVelocity).absoluteValue <= FlywheelConstants.FLYWHEEL_TOLERANCE - ) + ) || inputs.isSimulated var currentState = Companion.FlywheelStates.UNINITIALIZED @@ -153,7 +160,6 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { // left needs to be half of the right one flywheelLeftTargetVelocity = value.flywheelVelocity } - else -> {} } field = value } @@ -193,13 +199,13 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { ) } - io.configPID(kP.get(), kI.get(), kD.get()) + io.configPID(kP.get(), kI.get(), kD.get(), flywheelkV.get()) } override fun periodic() { io.updateInputs(inputs) Logger.processInputs("Flywheel", inputs) - Logger.recordOutput( + DebugLogger.recordDebugOutput( "Flywheel/targetDifference", (inputs.leftFlywheelVelocity - flywheelLeftTargetVelocity) .absoluteValue @@ -223,8 +229,8 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastRightFlywheelVoltage.inVolts) } - if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { - io.configPID(kP.get(), kI.get(), kD.get()) + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged() || flywheelkV.hasChanged()) { + io.configPID(kP.get(), kI.get(), kD.get(), flywheelkV.get()) } if (flywheelkA.hasChanged() || flywheelkV.hasChanged() || flywheelkS.hasChanged()) { @@ -244,7 +250,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { nextState = fromRequestToState(currentRequest) } Companion.FlywheelStates.TARGETING_VELOCITY -> { - setFlywheelVelocity(flywheelRightTargetVelocity) + setFlywheelVelocity(flywheelLeftTargetVelocity) lastFlywheelRunTime = Clock.fpgaTime nextState = fromRequestToState(currentRequest) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt index f75b7bd9..46560dc3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt @@ -13,6 +13,7 @@ import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.VelocityFeedforward import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.inNewtons import org.team4099.lib.units.derived.inVolts @@ -40,7 +41,7 @@ interface FlywheelIO { var leftFlywheelDutyCycle = 0.0.volts var leftFlywheelTorque = 0.0.newtons - var isSimulated = false + var isSimulated = true override fun toLog(table: LogTable) { table.put("flywheelRightVelocityRPM", rightFlywheelVelocity.inRotationsPerMinute) @@ -121,5 +122,6 @@ interface FlywheelIO { kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>, + kV: VelocityFeedforward ) {} } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt index a3b2fce2..428cdccf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt @@ -16,6 +16,7 @@ import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.VelocityFeedforward import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.inKilogramsMeterSquared import org.team4099.lib.units.derived.inVolts @@ -103,6 +104,7 @@ object FlywheelIOSim : FlywheelIO { kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>, + kV: VelocityFeedforward ) { flywheelController.setPID(kP, kI, kD) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt index affd73dc..f3b00ed8 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -6,8 +6,10 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.Follower +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage import com.ctre.phoenix6.controls.VoltageOut import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.lib.phoenix6.VelocityVoltage import com.team4099.robot2023.config.constants.Constants @@ -17,23 +19,26 @@ import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.util.DebugLogger +import org.littletonrobotics.junction.Logger import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.VelocityFeedforward import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.newtons +import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perSecond object FlywheelIOTalon : FlywheelIO { @@ -75,6 +80,7 @@ object FlywheelIOTalon : FlywheelIO { var rightFlywheelDutyCycle: StatusSignal var motorVoltage: StatusSignal var motorTorque: StatusSignal + var leftWheelClosedLoopTarget: StatusSignal init { @@ -84,43 +90,49 @@ object FlywheelIOTalon : FlywheelIO { flywheelLeftTalon.clearStickyFaults() flywheelRightTalon.clearStickyFaults() - flywheelRightConfiguration.Slot0.kP = - flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP) - flywheelRightConfiguration.Slot0.kI = - flywheelRightSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI) - flywheelRightConfiguration.Slot0.kD = - flywheelRightSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD) + flywheelLeftConfiguration.Slot0.kP = + flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP) + flywheelLeftConfiguration.Slot0.kI = + flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI) + flywheelLeftConfiguration.Slot0.kD = + flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD) + flywheelLeftConfiguration.Slot0.kV = + flywheelLeftSensor.velocityFeedforwardToRawUnits(FlywheelConstants.PID.REAL_FLYWHEEL_KV) + flywheelLeftConfiguration.Slot0.kA = FlywheelConstants.PID.REAL_FLYWHEEL_KA.value // // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) - flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = - FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes - flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = - FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes - flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = - FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds - flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = - FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes - flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false - - flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimit = - FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes - flywheelRightConfiguration.CurrentLimits.SupplyCurrentThreshold = - FlywheelConstants.RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes - flywheelRightConfiguration.CurrentLimits.SupplyTimeThreshold = - FlywheelConstants.RIGHT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds - flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - flywheelRightConfiguration.CurrentLimits.StatorCurrentLimit = - FlywheelConstants.RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes - flywheelRightConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + // flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = + // FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + // flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = + // FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + // flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = + // FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + // flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + // flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = + // FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + // flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelLeftConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + + // flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimit = + // FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + // flywheelRightConfiguration.CurrentLimits.SupplyCurrentThreshold = + // FlywheelConstants.RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + // flywheelRightConfiguration.CurrentLimits.SupplyTimeThreshold = + // FlywheelConstants.RIGHT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + // flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + // flywheelRightConfiguration.CurrentLimits.StatorCurrentLimit = + // FlywheelConstants.RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + // flywheelRightConfiguration.CurrentLimits.StatorCurrentLimitEnable = false flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast flywheelRightConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast flywheelRightTalon.configurator.apply(flywheelRightConfiguration) + flywheelLeftTalon.configurator.apply(flywheelLeftConfiguration) - flywheelLeftTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true)) + flywheelRightTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_LEFT_MOTOR_ID, true)) rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent @@ -129,6 +141,9 @@ object FlywheelIOTalon : FlywheelIO { motorVoltage = flywheelRightTalon.motorVoltage motorTorque = flywheelRightTalon.torqueCurrent + leftWheelClosedLoopTarget = flywheelLeftTalon.closedLoopReferenceSlope + leftWheelClosedLoopTarget.setUpdateFrequency(1000.0) + leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp @@ -157,40 +172,62 @@ object FlywheelIOTalon : FlywheelIO { 110.celsius ) ) - MotorChecker.add( - "Shooter", - "Flywheel", - MotorCollection( - mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")), - FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, - 90.celsius, - FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) - ) } override fun configPID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> + kD: DerivativeGain, Volt>, + kV: VelocityFeedforward ) { - val PIDRightConfig = Slot0Configs() - PIDRightConfig.kP = flywheelRightSensor.proportionalVelocityGainToRawUnits(kP) - PIDRightConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(kI) - PIDRightConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(kD) - - flywheelRightTalon.configurator.apply(PIDRightConfig) + val PIDLeftConfig = Slot0Configs() + PIDLeftConfig.kP = flywheelLeftSensor.proportionalVelocityGainToRawUnits(kP) + PIDLeftConfig.kI = flywheelLeftSensor.integralVelocityGainToRawUnits(kI) + PIDLeftConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(kD) + PIDLeftConfig.kV = flywheelLeftSensor.velocityFeedforwardToRawUnits(kV) + PIDLeftConfig.kA = FlywheelConstants.PID.REAL_FLYWHEEL_KA.value + + flywheelLeftTalon.configurator.apply(PIDLeftConfig) } override fun setFlywheelVoltage(voltage: ElectricalPotential) { - flywheelRightTalon.setControl(VoltageOut(voltage.inVolts)) + flywheelLeftTalon.setControl(VoltageOut(voltage.inVolts)) } override fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) { - velocityRequest.setFeedforward(feedforward) - velocityRequest.setVelocity(velocity) - flywheelRightTalon.setControl(velocityRequest.velocityVoltagePhoenix6) + val error = velocity - flywheelLeftSensor.velocity + + DebugLogger.recordDebugOutput( + "Flywheel/premotionTargetedVelocity", velocity.inRotationsPerMinute + ) + DebugLogger.recordDebugOutput( + "Flywheel/motionTargetedVelocity", flywheelLeftSensor.velocityToRawUnits(velocity) + ) + // flywheelRightTalon.setControl( + // VelocityVoltage( + // flywheelRightSensor.velocityToRawUnits(velocity), + // flywheelRightSensor.accelerationToRawUnits(0.0.radians.perSecond.perSecond), + // true, + // feedforward.inVolts, + // 0,1 + // false,1 + // false, + // false + // ) + // ) + + flywheelLeftTalon.setControl( + MotionMagicVelocityVoltage( + flywheelLeftSensor.velocityToRawUnits(velocity), + flywheelLeftSensor.accelerationToRawUnits(5000.rotations.perSecond.perSecond), + false, + 0.0.volts.inVolts, + 0, + true, + false, + false + ) + ) } private fun updateSignals() { @@ -213,6 +250,7 @@ object FlywheelIOTalon : FlywheelIO { updateSignals() + inputs.isSimulated = false inputs.rightFlywheelVelocity = flywheelRightSensor.velocity inputs.rightFlywheelAppliedVoltage = motorVoltage.value.volts inputs.rightFlywheelStatorCurrent = rightFlywheelStatorCurrentSignal.value.amps @@ -227,6 +265,10 @@ object FlywheelIOTalon : FlywheelIO { inputs.leftFlywheelStatorCurrent = leftFlywheelStatorCurrentSignal.value.amps inputs.leftFlywheelSupplyCurrent = leftFlywheelSupplyCurrentSignal.value.amps inputs.leftFlywheelTemperature = leftFlywheelTempSignal.value.celsius + inputs.leftFlywheelTorque = motorTorque.value.newtons + + leftWheelClosedLoopTarget.refresh() + Logger.recordOutput("Flywheel/profile", leftWheelClosedLoopTarget.value) } override fun setFlywheelBrakeMode(brake: Boolean) { @@ -238,6 +280,6 @@ object FlywheelIOTalon : FlywheelIO { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } - flywheelRightTalon.configurator.apply(motorOutputConfig) + flywheelLeftTalon.configurator.apply(motorOutputConfig) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 1e4c6e43..015ae4ef 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -5,6 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -108,7 +109,9 @@ class Intake(val io: IntakeIO) : SubsystemBase() { Logger.recordOutput( "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) ) - Logger.recordOutput("Intake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) + DebugLogger.recordDebugOutput( + "Intake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds + ) Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts) Logger.recordOutput("Intake/centerWheelVoltageTarget", centerWheelVoltageTarget.inVolts) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt new file mode 100644 index 00000000..50c171c3 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt @@ -0,0 +1,181 @@ +package com.team4099.robot2023.subsystems.intake + +import com.ctre.phoenix6.BaseStatusSignal +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.VoltageOut +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.IntakeConstants +import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.Neo +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.absoluteValue + +object IntakeIOFalconNEO : IntakeIO { + private val rollerFalcon = TalonFX(Constants.Intake.ROLLER_MOTOR_ID, "rio") + + private val rollerSensor = + ctreAngularMechanismSensor( + rollerFalcon, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION + ) + + private val centerWheelSparkMax = + CANSparkMax(Constants.Intake.CENTER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val centerWheelSensor = + sparkMaxAngularMechanismSensor( + centerWheelSparkMax, + IntakeConstants.CENTER_WHEEL_GEAR_RATIO, + IntakeConstants.VOLTAGE_COMPENSATION + ) + + var rollerAppliedVoltageSignal: StatusSignal + var rollerStatorCurrentSignal: StatusSignal + var rollerSupplyCurrentSignal: StatusSignal + var rollerTempSignal: StatusSignal + + init { + rollerFalcon.configurator + + var rollerFalconConfiguration = TalonFXConfiguration() + + rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = + IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes + rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = + IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes + rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = false + rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + rollerFalconConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast + + rollerFalcon.configurator.apply(rollerFalconConfiguration) + + rollerAppliedVoltageSignal = rollerFalcon.motorVoltage + rollerStatorCurrentSignal = rollerFalcon.statorCurrent + rollerSupplyCurrentSignal = rollerFalcon.supplyCurrent + rollerTempSignal = rollerFalcon.deviceTemp + + MotorChecker.add( + "Intake", + "Roller", + MotorCollection( + mutableListOf(Falcon500(rollerFalcon, "Roller Motor")), + IntakeConstants.ROLLER_CURRENT_LIMIT, + 60.celsius, + 50.amps, + 120.celsius + ), + ) + + centerWheelSparkMax.restoreFactoryDefaults() + centerWheelSparkMax.clearFaults() + + centerWheelSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) + centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) + centerWheelSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED + + centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + + centerWheelSparkMax.burnFlash() + + MotorChecker.add( + "Intake", + "Center Wheel", + MotorCollection( + mutableListOf(Neo(centerWheelSparkMax, "Center Wheel Motor")), + IntakeConstants.ROLLER_CURRENT_LIMIT, + 60.celsius, + 50.amps, + 120.celsius + ), + ) + } + + fun updateSignals() { + BaseStatusSignal.refreshAll( + rollerAppliedVoltageSignal, + rollerStatorCurrentSignal, + rollerSupplyCurrentSignal, + rollerTempSignal + ) + } + + override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { + updateSignals() + + inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerAppliedVoltageSignal.value.volts + inputs.rollerStatorCurrent = rollerStatorCurrentSignal.value.amps + inputs.rollerSupplyCurrent = rollerSupplyCurrentSignal.value.amps + inputs.rollerTemp = rollerTempSignal.value.celsius + + inputs.centerWheelVelocity = centerWheelSensor.velocity + + inputs.centerWheelAppliedVotlage = + centerWheelSparkMax.busVoltage.volts * centerWheelSparkMax.appliedOutput + inputs.centerWheelStatorCurrent = centerWheelSparkMax.outputCurrent.amps + + inputs.centerWheelSupplyCurrent = + inputs.centerWheelStatorCurrent * centerWheelSparkMax.appliedOutput.absoluteValue + inputs.centerWheelTemp = centerWheelSparkMax.motorTemperature.celsius + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setVoltage( + rollerVoltage: ElectricalPotential, + centerWheelVoltage: ElectricalPotential + ) { + rollerFalcon.setControl(VoltageOut(rollerVoltage.inVolts)) + + centerWheelSparkMax.setVoltage( + clamp( + centerWheelVoltage, + -IntakeConstants.VOLTAGE_COMPENSATION, + IntakeConstants.VOLTAGE_COMPENSATION + ) + .inVolts + ) + } + + /** + * Sets the roller motor brake mode + * + * @param brake if it brakes + */ + override fun setBrakeMode(rollerBrake: Boolean, centerWheelBrake: Boolean) { + var motorOutput = MotorOutputConfigs() + if (rollerBrake) { + motorOutput.NeutralMode = NeutralModeValue.Brake + } else { + motorOutput.NeutralMode = NeutralModeValue.Coast + } + + rollerFalcon.configurator.apply(motorOutput) + + if (centerWheelBrake) { + centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt index a526d8da..b2da5134 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt @@ -54,9 +54,9 @@ object IntakeIONEO : IntakeIO { MotorCollection( mutableListOf(Neo(rollerSparkMax, "Roller Motor")), IntakeConstants.ROLLER_CURRENT_LIMIT, - 70.celsius, - 30.amps, - 90.celsius + 60.celsius, + 50.amps, + 120.celsius ), ) @@ -77,9 +77,9 @@ object IntakeIONEO : IntakeIO { MotorCollection( mutableListOf(Neo(rollerSparkMax, "Center Wheel Motor")), IntakeConstants.ROLLER_CURRENT_LIMIT, - 70.celsius, - 30.amps, - 90.celsius + 60.celsius, + 50.amps, + 120.celsius ), ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt new file mode 100644 index 00000000..26ac3c09 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt @@ -0,0 +1,23 @@ +package com.team4099.robot2023.subsystems.led + +import com.team4099.robot2023.config.constants.LEDConstants +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs + +interface LedIO { + class LedIOInputs : LoggableInputs { + var ledState = LEDConstants.CandleState.NO_NOTE.toString() + + override fun toLog(table: LogTable?) { + table?.put("ledState", ledState.toString()) + } + + override fun fromLog(table: LogTable?) { + table?.getString("ledState", ledState.toString())?.let { ledState = it } + } + } + + fun setState(newState: LEDConstants.CandleState) {} + + fun updateInputs(inputs: LedIOInputs) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt new file mode 100644 index 00000000..e14e188b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -0,0 +1,32 @@ +package com.team4099.robot2023.subsystems.led + +import com.ctre.phoenix.led.CANdle +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.LEDConstants +import org.littletonrobotics.junction.Logger + +object LedIOCandle : LedIO { + + private val ledController = CANdle(Constants.LED.LED_CANDLE_ID) + private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE + + override fun updateInputs(inputs: LedIO.LedIOInputs) { + inputs.ledState = lastState.name + } + + override fun setState(newState: LEDConstants.CandleState) { + Logger.recordOutput("LED/newState", newState) + lastState = newState + setCANdleState(newState) + } + + private fun setCANdleState(state: LEDConstants.CandleState) { + if (state.animation == null) { + ledController.clearAnimation(0) + ledController.setLEDs(state.r, state.g, state.b) + } else { + ledController.animate(state.animation, 0) + ledController.setLEDs(state.r, state.g, state.b) + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 0511c685..7dd0f7b9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -1,241 +1,49 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. package com.team4099.robot2023.subsystems.led -import edu.wpi.first.math.MathUtil -import edu.wpi.first.wpilibj.AddressableLED -import edu.wpi.first.wpilibj.AddressableLEDBuffer +import com.team4099.robot2023.config.constants.LEDConstants +import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj.DriverStation.Alliance -import edu.wpi.first.wpilibj.Notifier -import edu.wpi.first.wpilibj.Timer -import edu.wpi.first.wpilibj.util.Color -import edu.wpi.first.wpilibj2.command.Subsystem -import kotlin.math.floor -import kotlin.math.pow -import kotlin.math.sin +import org.littletonrobotics.junction.Logger -class Leds : Subsystem { - // Robot state tracking - var loopCycleCount: Int = 0 - var hasNote = false - var isIdle = false - var alliance = DriverStation.getAlliance() - var lowBatteryAlert = false - var minLoopCycleCount = 10 - var length = 40 - private val breathDuration = 1.0 - private val waveExponent = 0.4 - private val waveFastCycleLength = 25.0 - private val waveFastDuration = 0.25 - private val waveSlowCycleLength = 25.0 - private val waveSlowDuration = 3.0 - private val waveAllianceCycleLength = 15.0 - private val waveAllianceDuration = 2.0 - - // LED IO - private val leds: AddressableLED - private val buffer: AddressableLEDBuffer - private val loadingNotifier: Notifier - - init { - println("[Init] Creating LEDs") - leds = AddressableLED(9) - buffer = AddressableLEDBuffer(length) - leds.setLength(length) - leds.setData(buffer) - leds.start() - loadingNotifier = - Notifier { - synchronized(this) { - breath( - Section.STATIC, - Color.kWhite, - Color.kBlack, - 0.25, - System.currentTimeMillis() / 1000.0 - ) - leds.setData(buffer) - } - } - loadingNotifier.startPeriodic(0.02) - } - - @Synchronized - override fun periodic() { - // Update alliance color - if (DriverStation.isFMSAttached()) { - alliance = DriverStation.getAlliance() - } - - // Exit during initial cycles - loopCycleCount += 1 - if (loopCycleCount < minLoopCycleCount) { - return - } - - // Stop loading notifier if running - loadingNotifier.stop() +class Leds(val io: LedIO) { + var inputs = LedIO.LedIOInputs() - // Select LED mode - - if (DriverStation.isDisabled()) { - if (lowBatteryAlert) { - // Low battery - solid(Section.FULL, Color.kOrangeRed) - } - // Default pattern - when (alliance.get()) { - Alliance.Red -> - wave( - Section.FULL, - Color.kRed, - Color.kBlack, - waveAllianceCycleLength, - waveAllianceDuration - ) - Alliance.Blue -> - wave( - Section.FULL, - Color.kBlue, - Color.kBlack, - waveAllianceCycleLength, - waveAllianceDuration - ) - else -> - wave(Section.FULL, Color.kGold, Color.kDarkBlue, waveSlowCycleLength, waveSlowDuration) - } - } else if (DriverStation.isAutonomous()) { - wave(Section.FULL, Color.kGold, Color.kBlack, waveFastCycleLength, waveFastDuration) - } else { - if (isIdle) { - if (hasNote) { - solid(Section.FULL, Color.kGreen) + var hasNote = false + var subsystemsAtPosition = false + var isIdle = true + var batteryIsLow = false + + var state = LEDConstants.CandleState.NO_NOTE + set(value) { + io.setState(value) + field = value + } + + fun periodic() { + io.updateInputs(inputs) + if (batteryIsLow && DriverStation.isDisabled()) { + state = LEDConstants.CandleState.LOW_BATTERY + } else if (DriverStation.isDisabled()) { + if (DriverStation.getAlliance().isPresent) { + if (FMSData.isBlue) { + state = LEDConstants.CandleState.BLUE } else { - solid(Section.FULL, Color.kGold) + state = LEDConstants.CandleState.RED } } else { - solid(Section.FULL, Color.kBlue) - } - } - - // Update LEDs - leds.setData(buffer) - } - - private fun solid(section: Section, color: Color?) { - if (color != null) { - for (i in section.start() until section.end()) { - buffer.setLED(i, color) - } - } - } - - private fun solid(percent: Double, color: Color) { - var i = 0 - while (i < MathUtil.clamp(length * percent, 0.0, length.toDouble())) { - buffer.setLED(i, color) - i++ - } - } - - private fun strobe(section: Section, color: Color?, duration: Double) { - val on = ((Timer.getFPGATimestamp() % duration) / duration) > 0.5 - solid(section, if (on) color else Color.kBlack) - } - - private fun breath( - section: Section, - c1: Color, - c2: Color, - duration: Double, - timestamp: Double = Timer.getFPGATimestamp(), - ) { - val x = ((timestamp % breathDuration) / breathDuration) * 2.0 * Math.PI - val ratio = (sin(x) + 1.0) / 2.0 - val red = (c1.red * (1 - ratio)) + (c2.red * ratio) - val green = (c1.green * (1 - ratio)) + (c2.green * ratio) - val blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio) - solid(section, Color(red, green, blue)) - } - - private fun rainbow(section: Section, cycleLength: Double, duration: Double) { - var x = (1 - ((Timer.getFPGATimestamp() / duration) % 1.0)) * 180.0 - val xDiffPerLed = 180.0 / cycleLength - for (i in 0 until section.end()) { - x += xDiffPerLed - x %= 180.0 - if (i >= section.start()) { - buffer.setHSV(i, x.toInt(), 255, 255) - } - } - } - - private fun wave(section: Section, c1: Color, c2: Color, cycleLength: Double, duration: Double) { - var x = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI - val xDiffPerLed = (2.0 * Math.PI) / cycleLength - for (i in 0 until section.end()) { - x += xDiffPerLed - if (i >= section.start()) { - var ratio: Double = (sin(x).pow(waveExponent) + 1.0) / 2.0 - if (java.lang.Double.isNaN(ratio)) { - ratio = (-sin(x + Math.PI).pow(waveExponent) + 1.0) / 2.0 - } - if (java.lang.Double.isNaN(ratio)) { - ratio = 0.5 - } - val red = (c1.red * (1 - ratio)) + (c2.red * ratio) - val green = (c1.green * (1 - ratio)) + (c2.green * ratio) - val blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio) - buffer.setLED(i, Color(red, green, blue)) + state = LEDConstants.CandleState.NOTHING } - } - } - - private fun stripes(section: Section, colors: List, length: Int, duration: Double) { - val offset = (Timer.getFPGATimestamp() % duration / duration * length * colors.size).toInt() - for (i in section.start() until section.end()) { - var colorIndex = (floor((i - offset).toDouble() / length) + colors.size).toInt() % colors.size - colorIndex = colors.size - 1 - colorIndex - buffer.setLED(i, colors[colorIndex]) - } - } - - private enum class Section { - STATIC, - FULL; - - fun start(): Int { - return when (this) { - STATIC -> 0 - FULL -> 0 - else -> 0 - } - } - - fun end(): Int { - return when (this) { - STATIC -> staticLength - FULL -> length + } else if (hasNote) { + if (subsystemsAtPosition && !isIdle) { + state = LEDConstants.CandleState.CAN_SHOOT + } else { + state = LEDConstants.CandleState.HAS_NOTE } + } else { + state = LEDConstants.CandleState.NO_NOTE } - } - - companion object { - var instance: Leds? = null - get() { - if (field == null) { - field = Leds() - } - return field - } - private set - private const val length = 43 - private const val staticLength = 14 + Logger.processInputs("LED", inputs) + Logger.recordOutput("LED/state", state.name) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 21379379..00d19361 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -1,10 +1,11 @@ package com.team4099.robot2023.subsystems.limelight -import com.team4099.lib.logging.TunableNumber +import com.team4099.lib.hal.Clock import com.team4099.lib.vision.TargetCorner -import com.team4099.lib.vision.TimestampedVisionUpdate +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.util.LimelightReading +import com.team4099.robot2023.util.findClosestPose import com.team4099.robot2023.util.rotateBy import com.team4099.robot2023.util.toPose3d import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -18,13 +19,17 @@ import org.team4099.lib.geometry.Translation2d import org.team4099.lib.geometry.Translation3d import org.team4099.lib.geometry.Translation3dWPILIB import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.tan -import java.util.function.Consumer import kotlin.math.hypot import kotlin.math.sqrt import kotlin.math.tan @@ -32,15 +37,83 @@ import kotlin.math.tan class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val inputs = LimelightVisionIO.LimelightVisionIOInputs() - var poseSupplier: () -> Pose2d = { Pose2d() } - var visionConsumer: Consumer> = Consumer {} - // i think we need this for camera project to irl coordinates val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan) val vph = (2.0 * (VisionConstants.Limelight.VERITCAL_FOV / 2).tan) - private val xyStdDevCoefficient = TunableNumber("LimelightVision/xystdev", 0.05) - private val thetaStdDev = TunableNumber("LimelightVision/thetaStdDev", 0.75) + var limelightState: LimelightStates = LimelightStates.UNINITIALIZED + + var targetGamePiecePose: Pose3d? = null + + var targetGamePieceTx: Angle? = null + + var poseSupplier: () -> Pose2d = { Pose2d() } + + var lastSeen: Time = -1337.seconds + + val visibleGamePieces = mutableListOf() + + lateinit var visibleGamePiecesTx: List + + enum class LimelightStates { + UNINITIALIZED, + IDLE, + TARGETING_GAME_PIECE + } + init {} + + override fun periodic() { + val startTime = Clock.realTimestamp + io.updateInputs(inputs) + Logger.processInputs("LimelightVision", inputs) + + var currentPose: Pose2d = poseSupplier.invoke() + + if (limelightState == LimelightStates.TARGETING_GAME_PIECE) { + if (inputs.validReading) { + for (target in inputs.gamePieceTargets) { + visibleGamePieces.add( + solveTargetPoseFromAngle(currentPose, target, FieldConstants.noteThickness / 2) + ) + } + + visibleGamePiecesTx = inputs.gamePieceTargets.map({ x -> x.tx }) + if (!visibleGamePieces.isEmpty()) { + targetGamePiecePose = + currentPose.toPose3d().findClosestPose(*visibleGamePieces.toTypedArray()) + targetGamePieceTx = visibleGamePiecesTx[visibleGamePieces.indexOf(targetGamePiecePose)] + lastSeen = Clock.fpgaTime + } else { + targetGamePieceTx = null + } + } + } + + Logger.recordOutput( + "LimelightVision/RawLimelightReadingsTx", + inputs.gamePieceTargets.map { it.tx.inDegrees }.toDoubleArray() + ) + + Logger.recordOutput( + "LimelightVision/RawLimelightReadingsTy", + inputs.gamePieceTargets.map { it.ty.inDegrees }.toDoubleArray() + ) + + Logger.recordOutput( + "LimelightVision/robotVisiblePieces", *visibleGamePieces.map { it.pose3d }.toTypedArray() + ) + + Logger.recordOutput( + "LimelightVision/cameraFieldRelativePose", + currentPose.toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM).pose3d + ) + + Logger.recordOutput( + "LoggedRobot/Subsystems/LimelightLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) + Logger.recordOutput("LimelightVision/LimeLightState", limelightState.name) + } fun solveTargetPoseFromAngle( currentPose: Pose2d, @@ -60,29 +133,18 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { distanceToTarget.inMeters, Rotation3dWPILIB(0.0, -target.ty.inRadians, -target.tx.inRadians) ) - Logger.recordOutput("LimelightVision/distanceToTarget", distanceToTarget.inMeters) - // figure out which way this target is facing using yaw of robot and yaw of camera - val targetRotation = - Rotation3d( - 0.0.degrees, - 0.0.degrees, - if (currentPose.rotation.rotateBy(VisionConstants.Limelight.LL_TRANSFORM.rotation.z) in - 0.degrees..180.degrees - ) { - // we are looking at a red node which is facing towards 180 degrees - 180.0.degrees - } else { - // we are looking at a blue node which is facing towards 0 degrees - 0.degrees - } - ) + val targetRotation = Rotation3d(0.0.degrees, 0.0.degrees, 0.0.degrees) return currentPose .toPose3d() .transformBy(VisionConstants.Limelight.LL_TRANSFORM) - .transformBy(Transform3d(Translation3d(targetTranslation), targetRotation)) + .transformBy( + Transform3d( + Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 0.0.degrees) + ) + ) } fun xyDistanceFromTarget(target: LimelightReading, targetHeight: Length): Length { @@ -107,6 +169,11 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { return targetToCameraHeight / tan(angleToGoal) } + fun angleYawFromTarget(currentPose: Pose2d, targetPose: Pose3d): Angle { + val robotToTarget = targetPose.toPose2d().relativeTo(currentPose) + return Math.asin((robotToTarget.y.inMeters / robotToTarget.translation.magnitude)).radians + } + // based off of angles fun solveTargetPositionFromAngularOutput( tx: Angle, @@ -124,6 +191,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val xDistanceFromTargetToCamera = (targetHeight - cameraTransform.z) / verticalAngleFromCamera.tan + val yDistanceFromTargetToCamera = xDistanceFromTargetToCamera * horizontalAngleFromCamera.tan val translationFromTargetToCamera = @@ -214,12 +282,4 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { null } } - - fun setDataInterfaces( - poseSupplier: () -> Pose2d, - visionConsumer: Consumer> - ) { - this.poseSupplier = poseSupplier - this.visionConsumer = visionConsumer - } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt index e39448bd..a12bf576 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt @@ -3,7 +3,9 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.robot2023.util.LimelightReading import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.base.Decimal import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.percent import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -15,50 +17,70 @@ interface LimelightVisionIO { var timestamp = 0.0.seconds var fps = 0.0 var validReading = false - var angle = 0.degrees - var retroTargets = listOf() + var gamePieceTargets = listOf() + var xAngle = 0.degrees + var yAngle = 0.degrees + var targetSize = 0.percent override fun fromLog(table: LogTable?) { - table?.get("timestampSeconds", timestamp.inSeconds)?.let { timestamp = it.seconds } - table?.get("fps", fps)?.let { fps = it } - table?.get("validReading", validReading)?.let { validReading = it } - table?.get("simpleAngleDegrees", angle.inDegrees)?.let { angle = it.degrees } - val numOfTargets = table?.get("numOfTargets", 0) ?: 0 + table?.getDouble("timestampSeconds", timestamp.inSeconds)?.let { timestamp = it.seconds } + table?.getDouble("fps", fps)?.let { fps = it } + table?.getBoolean("validReading", validReading)?.let { validReading = it } + table?.getDouble("xAngleDegrees", xAngle.inDegrees)?.let { xAngle = it.degrees } + table?.getDouble("yAngleDegrees", yAngle.inDegrees)?.let { yAngle = it.degrees } + table?.getDouble("targetSizePercent", targetSize.value)?.let { targetSize = it.percent } + val numOfTargets = table?.getInteger("numOfTargets", 0) ?: 0 val retrievedTargets = mutableListOf() for (targetIndex in 0 until numOfTargets) { - val targetTx: Angle? = table?.get("Detection/$targetIndex/tx", 0.0)?.degrees - val targetTy: Angle? = table?.get("Detection/$targetIndex/tx", 0.0)?.degrees - val targetTxPixels: Double? = table?.get("Detection/$targetIndex/txPixels", 0.0) - val targetTyPixels: Double? = table?.get("Detection/$targetIndex/tyPixels", 0.0) - val targetTs: Angle? = table?.get("Detection/$targetIndex/tyPixels", 0.0)?.degrees - if (targetTx != null && + val className: String? = table?.getString("Detection/$targetIndex/class", "") + val confidence: Decimal? = table?.getDouble("Detection/$targetIndex/conf", 0.0)?.percent + val targetTx: Angle? = table?.getDouble("Detection/$targetIndex/tx", 0.0)?.degrees + val targetTy: Angle? = table?.getDouble("Detection/$targetIndex/ty", 0.0)?.degrees + val targetTxPixels: Double? = table?.getDouble("Detection/$targetIndex/txp", 0.0) + val targetTyPixels: Double? = table?.getDouble("Detection/$targetIndex/typ", 0.0) + val targetTa: Decimal? = table?.getDouble("Detection/$targetIndex/ta", 0.0)?.percent + if ((className == "cone" || className == "cube") && + confidence != null && + targetTx != null && targetTy != null && targetTxPixels != null && targetTyPixels != null && - targetTs != null + targetTa != null ) { retrievedTargets.add( - LimelightReading(targetTx, targetTy, targetTxPixels, targetTyPixels, targetTs) + LimelightReading( + className, + confidence, + targetTx, + targetTy, + targetTxPixels, + targetTyPixels, + targetTa + ) ) } } - retroTargets = retrievedTargets.toList() + gamePieceTargets = retrievedTargets.toList() } override fun toLog(table: LogTable?) { table?.put("timestampSeconds", timestamp.inSeconds) table?.put("fps", fps) table?.put("validReading", validReading) - table?.put("simpleAngleDegrees", angle.inDegrees) - table?.put("numOfTargets", retroTargets.size.toLong()) - table?.put("cornersX", retroTargets.map { it.txPixel }.toDoubleArray()) - table?.put("cornersY", retroTargets.map { it.tyPixel }.toDoubleArray()) - for (i in retroTargets.indices) { - table?.put("Detection/$i/txDegrees", retroTargets[i].tx.inDegrees) - table?.put("Detection/$i/tyDegrees", retroTargets[i].ty.inDegrees) - table?.put("Detection/$i/tyPixels", retroTargets[i].tyPixel) - table?.put("Detection/$i/txPixels", retroTargets[i].txPixel) - table?.put("Detection/$i/tsDegrees", retroTargets[i].ts.inDegrees) + table?.getDouble("xAngleDegrees", xAngle.inDegrees) + table?.getDouble("yAngleDegrees", yAngle.inDegrees) + table?.getDouble("targetSizePercent", targetSize.value) + table?.put("numOfTargets", gamePieceTargets.size.toLong()) + table?.put("cornersX", gamePieceTargets.map { it.txPixel }.toDoubleArray()) + table?.put("cornersY", gamePieceTargets.map { it.tyPixel }.toDoubleArray()) + for (i in gamePieceTargets.indices) { + table?.put("Detection/$i/class", gamePieceTargets[i].className) + table?.put("Detection/$i/conf", gamePieceTargets[i].confidence.value) + table?.put("Detection/$i/tx", gamePieceTargets[i].tx.inDegrees) + table?.put("Detection/$i/ty", gamePieceTargets[i].ty.inDegrees) + table?.put("Detection/$i/typ", gamePieceTargets[i].tyPixel) + table?.put("Detection/$i/txp", gamePieceTargets[i].txPixel) + table?.put("Detection/$i/ta", gamePieceTargets[i].ta.value) } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt index 4df9cb42..3b9fa842 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt @@ -7,6 +7,7 @@ import com.team4099.utils.LimelightHelpers import edu.wpi.first.networktables.NetworkTableEntry import edu.wpi.first.networktables.NetworkTableInstance import org.team4099.lib.units.base.inMilliseconds +import org.team4099.lib.units.base.percent import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.milli @@ -25,8 +26,12 @@ object LimelightVisionIOReal : LimelightVisionIO { NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("cl") private val dataEntry: NetworkTableEntry = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tcornxy") - private val angleEntry: NetworkTableEntry = + private val xAngleEntry: NetworkTableEntry = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tx") + private val yAngleEntry: NetworkTableEntry = + NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ty") + private val targetSizeEntry: NetworkTableEntry = + NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ta") override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) { val totalLatency = @@ -36,12 +41,14 @@ object LimelightVisionIOReal : LimelightVisionIO { ) inputs.timestamp = Clock.realTimestamp - totalLatency - inputs.angle = angleEntry.getDouble(0.0).degrees + inputs.xAngle = xAngleEntry.getDouble(0.0).degrees + inputs.yAngle = yAngleEntry.getDouble(0.0).degrees + inputs.targetSize = (targetSizeEntry.getDouble(0.0) * 100).percent inputs.fps = 1000 / totalLatency.inMilliseconds inputs.validReading = true - inputs.retroTargets = - LimelightHelpers.getLatestResults(LIMELIGHT_NAME).targetingResults.targets_Retro.map { + inputs.gamePieceTargets = + LimelightHelpers.getLatestResults(LIMELIGHT_NAME).targetingResults.targets_Detector.map { LimelightReading(it) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt index 94d5a123..da2bd454 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt @@ -1,31 +1,48 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.lib.hal.Clock -import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.util.LimelightReading +import com.team4099.robot2023.util.rotateBy +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.percent import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.radians +import kotlin.math.acos +import kotlin.math.pow +import kotlin.math.sqrt object LimelightVisionIOSim : LimelightVisionIO { - private val tunableTx = - LoggedTunableValue( - "LimelightSim/txDegrees", (8.612154).degrees, Pair({ it.inDegrees }, { it.degrees }) - ) - private val tunableTy = - LoggedTunableValue( - "LimelightSim/tyDegrees", - (-1.6576093255536648).degrees, - Pair({ it.inDegrees }, { it.degrees }) - ) + var poseSupplier: () -> Pose2d = { Pose2d() } override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) { inputs.timestamp = Clock.realTimestamp - inputs.angle = 0.0.radians + inputs.xAngle = 0.0.radians + inputs.yAngle = 0.0.radians + inputs.targetSize = 0.0.percent inputs.fps = 90.0 inputs.validReading = true - inputs.retroTargets = - listOf(LimelightReading(tunableTx.get(), tunableTy.get(), 0.0, 0.0, 10.degrees)) + + inputs.gamePieceTargets = + listOf( + LimelightReading("cone", 0.0.percent, 0.0.degrees, 0.0.degrees, 0.0, 0.0, 0.0.percent) + ) + } + + private fun cartesianToSpherical(translationInCameraSpace: Translation3d): Rotation3d { + val x = translationInCameraSpace.x + val y = translationInCameraSpace.y + val z = translationInCameraSpace.z + + val r = sqrt(x.inMeters.pow(2) + y.inMeters.pow(2) + z.inMeters.pow(2)).meters + val theta = acos(z / r).radians.rotateBy(-VisionConstants.Limelight.LL_TRANSFORM.rotation.y) + val phi = (y.sign * acos(x.inMeters / sqrt(x.inMeters.pow(2) + y.inMeters.pow(2)))).radians + + return Rotation3d(phi, theta, 0.0.degrees) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt new file mode 100644 index 00000000..b897f273 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt @@ -0,0 +1,198 @@ +package com.team4099.robot2023.subsystems.superstructure + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FieldConstants +import com.team4099.robot2023.config.constants.SuperstructureConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.vision.Vision +import com.team4099.robot2023.util.AllianceFlipUtil +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap +import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.inInches +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.inRotationsPerMinute +import org.team4099.lib.units.perMinute +import kotlin.math.absoluteValue +import kotlin.math.hypot + +class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { + val flywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() + val wristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() + + val tunableFlywheelInterpolationTable: + List, LoggedTunableValue>>> + + val tunableWristInterpolationTable: + List, LoggedTunableValue>> + + val interpolationTestDistance = + LoggedTunableValue("AutoAim/TestDistance", 0.0.meters, Pair({ it.inInches }, { it.inches })) + + init { + + if (RobotBase.isReal()) { + tunableFlywheelInterpolationTable = + SuperstructureConstants.distanceFlywheelSpeedTableReal.mapIndexed { i, it -> + Pair( + LoggedTunableValue( + "AutoAim/FlywheelInterpolation/$i/Distance", + it.first, + Pair({ it.inInches }, { it.inches }) + ), + LoggedTunableValue( + "AutoAim/FlywheelInterpolation/$i/SpeedRPM", + it.second, + Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) + ) + ) + } + + tunableWristInterpolationTable = + SuperstructureConstants.distanceWristAngleTableReal.mapIndexed { i, it -> + Pair( + LoggedTunableValue( + "AutoAim/WristInterpolation/$i/Distance", + it.first, + Pair({ it.inInches }, { it.inches }) + ), + LoggedTunableValue( + "AutoAim/WristInterpolation/$i/AngleDegrees", + it.second, + Pair({ it.inDegrees }, { it.degrees }) + ) + ) + } + } else { + tunableFlywheelInterpolationTable = + SuperstructureConstants.distanceFlywheelSpeedTableSim.mapIndexed { i, it -> + Pair( + LoggedTunableValue( + "AutoAim/FlywheelInterpolation/$i/Distance", + it.first, + Pair({ it.inInches }, { it.inches }) + ), + LoggedTunableValue( + "AutoAim/FlywheelInterpolation/$i/SpeedRPM", + it.second, + Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) + ) + ) + } + + tunableWristInterpolationTable = + SuperstructureConstants.distanceWristAngleTableSim.mapIndexed { i, it -> + Pair( + LoggedTunableValue( + "AutoAim/WristInterpolation/$i/Distance", + it.first, + Pair({ it.inInches }, { it.inches }) + ), + LoggedTunableValue( + "AutoAim/WristInterpolation/$i/AngleDegrees", + it.second, + Pair({ it.inDegrees }, { it.degrees }) + ) + ) + } + } + + updateFlywheelInterpolationTable() + updateWristInterpolationTable() + } + + fun periodic() { + for (point in tunableFlywheelInterpolationTable) { + if (point.first.hasChanged() || point.second.hasChanged()) { + updateFlywheelInterpolationTable() + break + } + } + + for (point in tunableWristInterpolationTable) { + if (point.first.hasChanged() || point.second.hasChanged()) { + updateWristInterpolationTable() + break + } + } + + Logger.recordOutput( + "AutoAim/InterpolatedFlywheelSpeed", + flywheelSpeedRPMInterpolationTable.get(interpolationTestDistance.get().inMeters) + ) + + Logger.recordOutput( + "AutoAim/InterpolatedWristAngle", + wristAngleDegreesInterpolationTable.get(interpolationTestDistance.get().inMeters) + ) + } + + fun calculateFlywheelSpeed(): AngularVelocity { + return flywheelSpeedRPMInterpolationTable.get(calculateDistanceFromSpeaker().inMeters) + .rotations + .perMinute + } + + fun calculateWristAngle(): Angle { + return wristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees + } + + fun updateFlywheelInterpolationTable() { + flywheelSpeedRPMInterpolationTable.clear() + tunableFlywheelInterpolationTable.forEach { + flywheelSpeedRPMInterpolationTable.put( + it.first.get().inMeters, it.second.get().inRotationsPerMinute + ) + } + } + + fun updateWristInterpolationTable() { + wristAngleDegreesInterpolationTable.clear() + tunableWristInterpolationTable.forEach { + wristAngleDegreesInterpolationTable.put(it.first.get().inMeters, it.second.get().inDegrees) + } + } + + fun calculateDistanceFromSpeaker(): Length { + val distance = + if (DriverStation.isAutonomous()) { + val speakerTransformWithOdometry = + drivetrain.fieldTRobot.relativeTo( + AllianceFlipUtil.apply(FieldConstants.centerSpeakerOpening) + ) + Logger.recordOutput( + "AutoAim/speakerTransformWithOdometry", speakerTransformWithOdometry.pose2d + ) + hypot(speakerTransformWithOdometry.x.inMeters, speakerTransformWithOdometry.y.inMeters) + .meters + } else { + Translation2d( + vision.robotTSpeaker.y - + (drivetrain.robotVelocity.y * vision.robotTSpeaker.norm.absoluteValue / 5) + .value + .meters, + vision.robotTSpeaker.x - + (drivetrain.robotVelocity.x * vision.robotTSpeaker.norm.absoluteValue / 5) + .value + .meters + ) + .magnitude + .meters + } + Logger.recordOutput("AutoAim/currentDistanceInches", distance.inInches) + return distance + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index e2c777ab..2c5a2140 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.superstructure +import com.team4099.robot2023.config.constants.WristConstants import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity @@ -28,10 +29,17 @@ sealed interface Request { class PrepScoreSpeakerHigh() : SuperstructureRequest class ScoreSpeaker() : SuperstructureRequest - class ScoreSpeakerLow() : SuperstructureRequest class ScoreSpeakerMid() : SuperstructureRequest class ScoreSpeakerHigh() : SuperstructureRequest + class ManualScoreSpeakerPrep( + val wristAngle: Angle, + val flywheelVelocity: AngularVelocity, + val wristTolerance: Angle + ) : SuperstructureRequest + + class AutoAim() : SuperstructureRequest + class PrepTrap() : SuperstructureRequest class ScoreTrap() : SuperstructureRequest @@ -40,6 +48,8 @@ sealed interface Request { class ClimbRetract() : SuperstructureRequest + class PassingShot() : SuperstructureRequest + class Tuning() : SuperstructureRequest } @@ -60,6 +70,7 @@ sealed interface Request { class Idle : DrivetrainRequest class LockWheels : DrivetrainRequest + class Characterize(val voltage: ElectricalPotential) : DrivetrainRequest } sealed interface IntakeRequest : Request { @@ -81,7 +92,10 @@ sealed interface Request { } sealed interface WristRequest : Request { class OpenLoop(val wristVoltage: ElectricalPotential) : WristRequest - class TargetingPosition(val wristPosition: Angle) : WristRequest + class TargetingPosition( + val wristPosition: Angle, + val wristTolerance: Angle = WristConstants.WRIST_TOLERANCE + ) : WristRequest class Zero() : WristRequest } sealed interface FlywheelRequest : Request { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 1b9d59a9..0b60eca0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1,18 +1,24 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.hal.Clock +import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.config.constants.FieldConstants +import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.elevator.Elevator import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.flywheel.Flywheel import com.team4099.robot2023.subsystems.intake.Intake +import com.team4099.robot2023.subsystems.led.LedIOCandle import com.team4099.robot2023.subsystems.led.Leds +import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.util.NoteSimulation import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -20,13 +26,20 @@ import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform3d import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerMinute +import org.team4099.lib.units.perMinute class Superstructure( private val intake: Intake, @@ -34,12 +47,31 @@ class Superstructure( private val elevator: Elevator, private val wrist: Wrist, private val flywheel: Flywheel, - private val drivetrain: Drivetrain + private val drivetrain: Drivetrain, + private val vision: Vision ) : SubsystemBase() { - var leds = Leds() + var wristPushDownVoltage = Wrist.TunableWristStates + + var leds = Leds(LedIOCandle) + + var aimer = AutoAim(drivetrain, vision) + + var cleanupStartTime = Clock.fpgaTime + + private var wristAngleToShootAt = 0.0.degrees + private var flywheelToShootAt = 0.0.rotations.perMinute var currentRequest: Request.SuperstructureRequest = Request.SuperstructureRequest.Idle() + set(value) { + when (value) { + is Request.SuperstructureRequest.ManualScoreSpeakerPrep -> { + wristAngleToShootAt = value.wristAngle + flywheelToShootAt = value.flywheelVelocity + } + } + field = value + } var currentState: SuperstructureStates = SuperstructureStates.UNINITIALIZED @@ -103,14 +135,30 @@ class Superstructure( notes.forEach { it.wristAngleSupplier = { wrist.inputs.wristPosition } } notes.forEach { it.elevatorHeightSupplier = { elevator.inputs.elevatorPosition } } notes.forEach { it.flywheelAngularVelocitySupplier = { flywheel.inputs.rightFlywheelVelocity } } + notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT } override fun periodic() { leds.hasNote = feeder.hasNote leds.isIdle = currentState == SuperstructureStates.IDLE + leds.subsystemsAtPosition = + wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition + leds.batteryIsLow = + RobotController.getBatteryVoltage() < LEDConstants.BATTERY_WARNING_THRESHOLD.inVolts + + val ledLoopStartTime = Clock.realTimestamp + leds.periodic() + Logger.recordOutput( + "LoggedRobot/Subsystems/LEDLoopTimeMS", + (Clock.realTimestamp - ledLoopStartTime).inMilliseconds + ) - notes.forEach { it.periodic() } - notes.forEach { Logger.recordOutput("NoteSimulation/${it.id}", toDoubleArray(it.currentPose)) } + if (!RobotBase.isReal()) { + notes.forEach { it.periodic() } + notes.forEach { + Logger.recordOutput("NoteSimulation/${it.id}", toDoubleArray(it.currentPose)) + } + } Logger.recordOutput( "SimulatedMechanisms/0", @@ -217,6 +265,7 @@ class Superstructure( noteHoldingID = 0 } SuperstructureStates.IDLE -> { + intake.currentRequest = Request.IntakeRequest.OpenLoop( Intake.TunableIntakeStates.idleRollerVoltage.get(), @@ -226,17 +275,10 @@ class Superstructure( feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.idleVoltage.get()) - if (DriverStation.isAutonomous()) { - flywheel.currentRequest = - Request.FlywheelRequest.TargetingVelocity( - Flywheel.TunableFlywheelStates.speakerVelocity.get() - ) - } else { - flywheel.currentRequest = - Request.FlywheelRequest.TargetingVelocity( - Flywheel.TunableFlywheelStates.idleVelocity.get() - ) - } + flywheel.currentRequest = + Request.FlywheelRequest.TargetingVelocity( + Flywheel.TunableFlywheelStates.idleVelocity.get() + ) if (DriverStation.isAutonomous()) { wrist.currentRequest = @@ -269,13 +311,17 @@ class Superstructure( nextState = SuperstructureStates.EJECT_GAME_PIECE_PREP } is Request.SuperstructureRequest.PrepScoreAmp -> { - nextState = SuperstructureStates.SCORE_AMP_PREP - } - is Request.SuperstructureRequest.ScoreAmp -> { - nextState = SuperstructureStates.SCORE_AMP + val currentRotation = drivetrain.odomTRobot.rotation + if ((currentRotation > 0.0.degrees && currentRotation < 180.degrees)) { + nextState = SuperstructureStates.ELEVATOR_AMP_PREP + } else { + nextState = SuperstructureStates.WRIST_AMP_PREP + } } is Request.SuperstructureRequest.ScoreSpeaker -> { - nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + if (feeder.hasNote) { + nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } } is Request.SuperstructureRequest.PrepScoreSpeakerLow -> { nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP @@ -298,10 +344,22 @@ class Superstructure( is Request.SuperstructureRequest.Tuning -> { nextState = SuperstructureStates.TUNING } + is Request.SuperstructureRequest.AutoAim -> { + if (feeder.hasNote) { + nextState = SuperstructureStates.AUTO_AIM + } + } + is Request.SuperstructureRequest.ManualScoreSpeakerPrep -> { + nextState = SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP + } + is Request.SuperstructureRequest.PassingShot -> { + nextState = SuperstructureStates.PASSING_SHOT_PREP + } } } SuperstructureStates.GROUND_INTAKE_PREP -> { - wrist.currentRequest = Request.WristRequest.OpenLoop(-2.volts) + wrist.currentRequest = + Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.intakeAngle.get()) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.GROUND_INTAKE } @@ -313,11 +371,18 @@ class Superstructure( } } SuperstructureStates.GROUND_INTAKE -> { + wrist.currentRequest = + Request.WristRequest.OpenLoop(Wrist.TunableWristStates.pushDownVoltage.get()) intake.currentRequest = Request.IntakeRequest.OpenLoop( Intake.TunableIntakeStates.intakeRollerVoltage.get(), Intake.TunableIntakeStates.intakeCenterWheelVoltage.get() ) + + if (DriverStation.isTeleop()) { + flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(-6.volts) + } + if (noteHoldingID == -1) { for (note in notes) { if (note.canIntake()) { @@ -328,19 +393,20 @@ class Superstructure( } } - if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) { - currentRequest = Request.SuperstructureRequest.Idle() - nextState = SuperstructureStates.IDLE - } - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake( if (DriverStation.isAutonomous()) Feeder.TunableFeederStates.autoIntakeVoltage.get() else Feeder.TunableFeederStates.intakeVoltage.get() ) - if (feeder.hasNote) { - currentRequest = Request.SuperstructureRequest.Idle() - nextState = SuperstructureStates.IDLE + + if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) { + if (DriverStation.isTeleop()) { + cleanupStartTime = Clock.fpgaTime + nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP + } else { + currentRequest = Request.SuperstructureRequest.Idle() + nextState = SuperstructureStates.IDLE + } } when (currentRequest) { is Request.SuperstructureRequest.Idle -> { @@ -352,9 +418,40 @@ class Superstructure( is Request.SuperstructureRequest.ScoreSpeaker -> { nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP } + is Request.SuperstructureRequest.AutoAim -> { + nextState = SuperstructureStates.AUTO_AIM + } + } + } + SuperstructureStates.GROUND_INTAKE_CLEAN_UP -> { + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.volts) + + if (Clock.fpgaTime - cleanupStartTime > FeederConstants.CLEAN_UP_TIME) { + currentRequest = Request.SuperstructureRequest.Idle() + nextState = SuperstructureStates.IDLE } } - SuperstructureStates.SCORE_AMP_PREP -> { + SuperstructureStates.AUTO_AIM -> { + val targetFlywheelSpeed = aimer.calculateFlywheelSpeed() + val targetWristAngle = aimer.calculateWristAngle() + + Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute) + Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees) + + flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) + wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) + + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + is Request.SuperstructureRequest.ScoreSpeaker -> { + nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime + } + } + } + SuperstructureStates.ELEVATOR_AMP_PREP -> { elevator.currentRequest = Request.ElevatorRequest.TargetingPosition( Elevator.TunableElevatorHeights.shootAmpPosition.get() @@ -366,7 +463,7 @@ class Superstructure( wrist.isAtTargetedPosition && currentRequest is Request.SuperstructureRequest.ScoreAmp ) { - nextState = SuperstructureStates.SCORE_AMP + nextState = SuperstructureStates.SCORE_ELEVATOR_AMP shootStartTime = Clock.fpgaTime } @@ -376,7 +473,20 @@ class Superstructure( } } } - SuperstructureStates.SCORE_AMP -> { + SuperstructureStates.WRIST_AMP_PREP -> { + wrist.currentRequest = + Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.fastAmpAngle.get()) + flywheel.currentRequest = + Request.FlywheelRequest.TargetingVelocity( + Flywheel.TunableFlywheelStates.ampVelocity.get() + ) + when (currentRequest) { + is Request.SuperstructureRequest.ScoreAmp -> { + nextState = SuperstructureStates.SCORE_SPEAKER + } + } + } + SuperstructureStates.SCORE_ELEVATOR_AMP -> { if (noteHoldingID != -1) { notes[noteHoldingID].currentState = NoteSimulation.NoteStates.AMP_SCORE @@ -386,9 +496,7 @@ class Superstructure( flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(-10.volts) feeder.currentRequest = Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.outtakeVoltage.get()) - if (!feeder.hasNote && - Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get() - ) { + if (Clock.fpgaTime - shootStartTime > Flywheel.TunableFlywheelStates.ampScoreTime.get()) { currentRequest = Request.SuperstructureRequest.Idle() } @@ -417,12 +525,16 @@ class Superstructure( currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime } when (currentRequest) { is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE } + is Request.SuperstructureRequest.AutoAim -> { + nextState = SuperstructureStates.AUTO_AIM + } } } SuperstructureStates.SCORE_SPEAKER -> { @@ -439,12 +551,16 @@ class Superstructure( ) { currentRequest = Request.SuperstructureRequest.Idle() + nextState = SuperstructureStates.IDLE } when (currentRequest) { is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE } + is Request.SuperstructureRequest.GroundIntake -> { + nextState = SuperstructureStates.GROUND_INTAKE_PREP + } } } SuperstructureStates.SCORE_SPEAKER_MID_PREP -> { @@ -466,6 +582,7 @@ class Superstructure( currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime } when (currentRequest) { @@ -493,6 +610,24 @@ class Superstructure( currentRequest is Request.SuperstructureRequest.ScoreSpeaker ) { nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime + } + + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } + } + SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP -> { + flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(flywheelToShootAt) + wrist.currentRequest = Request.WristRequest.TargetingPosition(wristAngleToShootAt) + if (wrist.isAtTargetedPosition && + flywheel.isAtTargetedVelocity && + currentRequest is Request.SuperstructureRequest.ScoreSpeaker + ) { + nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime } when (currentRequest) { @@ -508,6 +643,7 @@ class Superstructure( currentRequest is Request.SuperstructureRequest.ScoreTrap ) { nextState = SuperstructureStates.SCORE_TRAP + shootStartTime = Clock.fpgaTime } when (currentRequest) { @@ -565,6 +701,7 @@ class Superstructure( ) feeder.currentRequest = Request.FeederRequest.OpenLoopShoot(Feeder.TunableFeederStates.outtakeVoltage.get()) + flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(-10.volts) when (currentRequest) { is Request.SuperstructureRequest.Idle -> { @@ -575,7 +712,7 @@ class Superstructure( } SuperstructureStates.EJECT_GAME_PIECE_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) + Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.ejectAngle.get()) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.EJECT_GAME_PIECE @@ -587,6 +724,28 @@ class Superstructure( } } } + SuperstructureStates.PASSING_SHOT_PREP -> { + wrist.currentRequest = + Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.passingShotAngle.get()) + flywheel.currentRequest = + Request.FlywheelRequest.TargetingVelocity( + Flywheel.TunableFlywheelStates.passingShotVelocity.get() + ) + + shootStartTime = Clock.fpgaTime + + if (flywheel.isAtTargetedVelocity && + currentRequest is Request.SuperstructureRequest.ScoreSpeaker + ) { + nextState = SuperstructureStates.SCORE_SPEAKER + } + + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } + } SuperstructureStates.TUNING -> { if (currentRequest is Request.SuperstructureRequest.Idle) { nextState = SuperstructureStates.IDLE @@ -629,12 +788,26 @@ class Superstructure( } fun groundIntakeCommand(): Command { - val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() } + val returnCommand = + run { currentRequest = Request.SuperstructureRequest.GroundIntake() }.until { + currentState == SuperstructureStates.GROUND_INTAKE_PREP || + currentState == SuperstructureStates.GROUND_INTAKE + } returnCommand.name = "GroundIntakeCommand" return returnCommand } + fun passingShotCommand(): Command { + val returnCommand = + runOnce { currentRequest = Request.SuperstructureRequest.PassingShot() }.until { + currentState == SuperstructureStates.PASSING_SHOT_PREP + } + + returnCommand.name = "PassingShotCommand" + return returnCommand + } + fun homeCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.Home() }.until { @@ -647,7 +820,11 @@ class Superstructure( fun prepAmpCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreAmp() }.until { - isAtRequestedState && currentState == SuperstructureStates.SCORE_AMP_PREP + isAtRequestedState && + ( + currentState == SuperstructureStates.ELEVATOR_AMP_PREP || + currentState == SuperstructureStates.WRIST_AMP_PREP + ) } returnCommand.name = "PrepAmpCommand" return returnCommand @@ -662,10 +839,31 @@ class Superstructure( return returnCommand } + fun prepManualSpeakerCommand( + wristAngle: Angle, + flywheelVelocity: AngularVelocity = FlywheelConstants.SPEAKER_VELOCITY, + wristTolerance: Angle = WristConstants.WRIST_TOLERANCE + ): Command { + val returnCommand = + run { + currentRequest = + Request.SuperstructureRequest.ManualScoreSpeakerPrep( + wristAngle, flywheelVelocity, wristTolerance + ) + } + .until { + isAtRequestedState && currentState == SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP + } + returnCommand.name = "PrepManualSpeakerCommand" + return returnCommand + } + fun scoreCommand(): Command { val returnCommand = - runOnce { - if (currentState == SuperstructureStates.SCORE_AMP_PREP) { + run { + if (currentState == SuperstructureStates.ELEVATOR_AMP_PREP || + currentState == SuperstructureStates.WRIST_AMP_PREP + ) { currentRequest = Request.SuperstructureRequest.ScoreAmp() } else if (currentState == SuperstructureStates.SCORE_TRAP_PREP) { currentRequest = Request.SuperstructureRequest.ScoreTrap() @@ -732,6 +930,15 @@ class Superstructure( return returnCommand } + fun autoAimCommand(): Command { + val returnCommand = + runOnce { currentRequest = Request.SuperstructureRequest.AutoAim() }.until { + isAtRequestedState && currentState == SuperstructureStates.AUTO_AIM + } + returnCommand.name = "AutoAim" + return returnCommand + } + fun tuningCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.Tuning() }.until { @@ -817,30 +1024,36 @@ class Superstructure( HOME, GROUND_INTAKE_PREP, GROUND_INTAKE, - SCORE_AMP_PREP, - SCORE_AMP, + GROUND_INTAKE_CLEAN_UP, + ELEVATOR_AMP_PREP, + WRIST_AMP_PREP, + SCORE_ELEVATOR_AMP, SCORE_SPEAKER_LOW_PREP, SCORE_SPEAKER_MID_PREP, SCORE_SPEAKER_HIGH_PREP, SCORE_SPEAKER, + MANUAL_SCORE_SPEAKER_PREP, SCORE_TRAP_PREP, SCORE_TRAP, CLIMB_EXTEND, CLIMB_RETRACT, EJECT_GAME_PIECE, EJECT_GAME_PIECE_PREP, + PASSING_SHOT_PREP, + PASSING_SHOT, + AUTO_AIM } } } - /* fun requestIdleCommand(): Command { - val returnCommand = runOnce{currentRequest = Request.SuperstructureRequest.Idle()}.until{} isAtRequestedState && currentState == SuperstructureStates.IDLE} - } - - fun ejectGamePieceCommand(): Command { - val returnCommand = runOnce { - currentRequest = Request.SuperstructureRequest.EjectGamePiece() - }.until (isAtRequestedState && currentState == SuperstructureStates.EJECT_GAME_PIECE ) - returnCommand.name = "EjectGamePieceCommand" - return returnCommand - }*/ +/* fun requestIdleCommand(): Command { + val returnCommand = runOnce{currentRequest = Request.SuperstructureRequest.Idle()}.until{} isAtRequestedState && currentState == SuperstructureStates.IDLE} +} + +fun ejectGamePieceCommand(): Command { + val returnCommand = runOnce { + currentRequest = Request.SuperstructureRequest.EjectGamePiece() + }.until (isAtRequestedState && currentState == SuperstructureStates.EJECT_GAME_PIECE ) + returnCommand.name = "EjectGamePieceCommand" + return returnCommand +}*/ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 3503eb8b..b98b9ea8 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -2,32 +2,42 @@ package com.team4099.robot2023.subsystems.vision import com.team4099.lib.hal.Clock import com.team4099.lib.logging.TunableNumber +import com.team4099.lib.logging.toDoubleArray +import com.team4099.lib.vision.TimestampedTrigVisionUpdate import com.team4099.lib.vision.TimestampedVisionUpdate -import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.vision.camera.CameraIO -import edu.wpi.first.math.VecBuilder +import com.team4099.robot2023.util.FMSData +import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger +import org.photonvision.PhotonUtils import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Pose3d -import org.team4099.lib.geometry.Pose3dWPILIB +import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians import java.util.function.Consumer import java.util.function.Supplier -import kotlin.math.pow class Vision(vararg cameras: CameraIO) : SubsystemBase() { val io: List = cameras.toList() val inputs = List(io.size) { CameraIO.CameraInputs() } + var drivetrainOdometry: () -> Pose2d = { Pose2d() } + var robotTSpeaker: Translation3d = Translation3d() + var trustedRobotDistanceToTarget: Length = 0.meters + companion object { val ambiguityThreshold = 0.7 val targetLogTime = 0.05.seconds @@ -43,6 +53,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { private var fieldFramePoseSupplier = Supplier { Pose2d() } private var visionConsumer: Consumer> = Consumer {} + private var speakerVisionConsumer: Consumer = Consumer {} private val lastFrameTimes = mutableMapOf() private val lastTagDetectionTimes = mutableMapOf() @@ -54,22 +65,15 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { fun setDataInterfaces( fieldFramePoseSupplier: Supplier, - visionConsumer: Consumer> + visionConsumer: Consumer>, + speakerVisionMeasurementConsumer: Consumer ) { this.fieldFramePoseSupplier = fieldFramePoseSupplier this.visionConsumer = visionConsumer + this.speakerVisionConsumer = speakerVisionMeasurementConsumer } override fun periodic() { - // val tuningPosition = Pose3d(Pose3d( - // (43.125).inches, - // (108.375).inches, - // (18.22).inches, - // Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) - // ).translation + (Translation3d(45.625.inches, 1.3125.inches, 0.0.inches)), - // Rotation3d()).toPose2d() - // - // Logger.recordOutput("Vision/tuningPosition", tuningPosition.pose2d) val startTime = Clock.realTimestamp @@ -78,8 +82,8 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { Logger.processInputs("Vision/${VisionConstants.CAMERA_NAMES[instance]}", inputs[instance]) } - var fieldTCurrentRobotEstimate: Pose2d = fieldFramePoseSupplier.get() - val robotPoses = mutableListOf() + val robotPoses = mutableListOf() + val robotDistancesToTarget = mutableListOf() val visionUpdates = mutableListOf() for (instance in io.indices) { @@ -89,7 +93,68 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val values = inputs[instance].frame var cameraPose: Pose3d? = inputs[instance].frame - var robotPose: Pose2d? = cameraPose?.transformBy(cameraPoses[instance])?.toPose2d() + var robotPose: Pose2d? = null + var robotDistanceToTarget: Length? = null + var tagTargets = inputs[instance].cameraTargets + + println(tagTargets.size) + + val cornerData = mutableListOf() + + for (tag in tagTargets) { + if (DriverStation.getAlliance().isPresent) { + if ((tag.fiducialId in intArrayOf(4) && !FMSData.isBlue) || + (tag.fiducialId in intArrayOf(7) && FMSData.isBlue) + ) { // i made the tag IDS up + + robotDistanceToTarget = + PhotonUtils.calculateDistanceToTargetMeters( + cameraPoses[instance].translation.z.inMeters, + 57.125.inches.inMeters, + 23.25.degrees.inRadians, + tag.pitch.degrees.inRadians + ) + .meters + + Logger.recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/robotDistanceToTarget", + robotDistanceToTarget.inMeters + ) + + var cameraTspeaker2d = + Translation2d( + PhotonUtils.estimateCameraToTargetTranslation( + robotDistanceToTarget.inMeters, Rotation2d(-tag.yaw.degrees.inRadians) + ) + ) + + robotTSpeaker = + Translation3d(cameraTspeaker2d.x + 4.inches, cameraTspeaker2d.y, 0.meters) + + val timestampedTrigVisionUpdate = + TimestampedTrigVisionUpdate( + inputs[instance].timestamp, + Transform2d(Translation2d(robotTSpeaker.x, robotTSpeaker.y), 0.0.degrees) + ) + speakerVisionConsumer.accept(timestampedTrigVisionUpdate) + + Logger.recordOutput( + "Vision/${VisionConstants.CAMERA_NAMES[instance]}/robotTspeaker", + robotTSpeaker.translation3d + ) + + for (corner in tag.detectedCorners) { + cornerData.add(corner.x) + cornerData.add(corner.y) + } + } + } + } + + Logger.recordOutput("Vision/cornerDetections/$instance}", cornerData.toDoubleArray()) + + robotPoses.add(robotPose) + robotDistancesToTarget.add(robotDistanceToTarget) if (cameraPose == null || robotPose == null) { continue @@ -100,75 +165,26 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { ) { continue } + } - // Find all detected tag poses - val tagPoses = inputs[instance].usedTargets.map { FieldConstants.fieldAprilTags[it].pose } - - // Calculate average distance to tag - var totalDistance = 0.0.meters - for (tagPose in tagPoses) { - totalDistance += tagPose.translation.getDistance(cameraPose.translation) - } - val averageDistance = totalDistance / tagPoses.size - - // Add to vision updates - val xyStdDev = xyStdDevCoefficient.get() * averageDistance.inMeters.pow(2) / tagPoses.size - val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size - - visionUpdates.add( - TimestampedVisionUpdate( - timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) - ) - ) - robotPoses.add(robotPose) + var trustedRobotPose: Pose2d? = Pose2d() - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS", - (Clock.fpgaTime - timestamp).inMilliseconds - ) - - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", robotPose.pose2d - ) - - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", - *tagPoses.map { it.pose3d }.toTypedArray() - ) - - if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", - Pose2dWPILIB.struct, - Pose2d().pose2d - ) + for (cameraInstance in VisionConstants.TRUSTED_CAMERA_ORDER) { + if (cameraInstance in io.indices && robotPoses[cameraInstance] != null) { + trustedRobotPose = robotPoses[cameraInstance] + break } + } - if (Clock.fpgaTime - lastFrameTimes[instance]!! > targetLogTime) { - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", - Pose3dWPILIB.struct, - *arrayOf() - ) + for (cameraInstance in VisionConstants.TRUSTED_CAMERA_ORDER) { + if (cameraInstance in io.indices && robotDistancesToTarget[cameraInstance] != null) { + trustedRobotDistanceToTarget = robotDistancesToTarget[cameraInstance]!! + break } - - val allTagPoses = mutableListOf() - // for (detectionEntry in lastTagDetectionTimes.entries) { - // if (Clock.fpgaTime - detectionEntry.value < targetLogTime) { - // FieldConstants.getTagPose(detectionEntry.key)?.let { allTagPoses.add(it) } - // } - // } - - Logger.recordOutput( - "Vision/allTagPoses", Pose3dWPILIB.struct, *allTagPoses.map { it.pose3d }.toTypedArray() - ) - - visionConsumer.accept(visionUpdates) - - Logger.recordOutput( - "LoggedRobot/Subsystems/VisionLoopTimeMS", - (Clock.realTimestamp - startTime).inMilliseconds - ) } + + Logger.recordOutput( + "LoggedRobot/VisionLoopTimeMS", (Clock.realTimestamp - startTime).inMilliseconds + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt index 557cb15c..d6b94881 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt @@ -1,5 +1,10 @@ package com.team4099.robot2023.subsystems.vision.camera +import edu.wpi.first.math.MatBuilder +import edu.wpi.first.math.Nat +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.math.numbers.N5 import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.photonvision.common.dataflow.structures.Packet @@ -19,12 +24,16 @@ interface CameraIO { var cameraTargets = mutableListOf() var indices = 0 val photonPacketSerde = APacketSerde() + var cameraMatrix = MatBuilder.fill(Nat.N3(), Nat.N3(), *DoubleArray(9) { 0.0 }) + var distCoeff = MatBuilder.fill(Nat.N5(), Nat.N1(), *DoubleArray(5) { 0.0 }) override fun toLog(table: LogTable?) { table?.put("timestampSeconds", timestamp.inSeconds) table?.put("frame", frame.pose3d) table?.put("fps", fps) table?.put("usedTargets", usedTargets.toIntArray()) + table?.put("cameraMatrix", cameraMatrix.data) + table?.put("distCoeff", distCoeff.data) for (targetID in cameraTargets.indices) { val photonPacket = Packet(photonPacketSerde.maxByteSize) @@ -43,6 +52,12 @@ interface CameraIO { table?.get("cameraTargets/indices", 0)?.let { indices = it } + table?.get("distCoeff", MatBuilder.fill(Nat.N5(), Nat.N1(), *DoubleArray(5) { 0.0 }).data) + ?.let { distCoeff = MatBuilder.fill(Nat.N5(), Nat.N1(), *it) } + + table?.get("cameraMatrix", MatBuilder.fill(Nat.N3(), Nat.N3(), *DoubleArray(9) { 0.0 }).data) + ?.let { cameraMatrix = MatBuilder.fill(Nat.N3(), Nat.N3(), *it) } + cameraTargets = mutableListOf() for (targetID in 0 until indices) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt index 07ad39c8..b1f99f07 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt @@ -35,9 +35,15 @@ class CameraIOPhotonvision(private val identifier: String) : CameraIO { } override fun updateInputs(inputs: CameraIO.CameraInputs) { + if (camera.isConnected) { + inputs.cameraMatrix = camera.cameraMatrix.get() + inputs.distCoeff = camera.distCoeffs.get() + } + val pipelineResult = camera.latestResult Logger.recordOutput("$identifier/timestampIG", pipelineResult.timestampSeconds) if (pipelineResult.hasTargets()) { + inputs.timestamp = pipelineResult.timestampSeconds.seconds Logger.recordOutput("$identifier/hasTarget", pipelineResult.hasTargets()) inputs.cameraTargets = pipelineResult.targets diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 198d968d..0d7813b4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -5,6 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -15,6 +16,7 @@ import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.degrees @@ -38,11 +40,27 @@ import org.team4099.lib.units.perSecond class Wrist(val io: WristIO) : SubsystemBase() { val inputs = WristIO.WristIOInputs() + val arbitraryFeedforward = + LoggedTunableValue("Wrist/arbitraryFeedforward", Pair({ it.inVolts }, { it.volts })) + object TunableWristStates { val idleAngle = LoggedTunableValue( "Wrist/idleAngle", WristConstants.IDLE_ANGLE, Pair({ it.inDegrees }, { it.degrees }) ) + + val ejectAngle = + LoggedTunableValue( + "Wrist/ejectAngle", WristConstants.EJECT_ANGLE, Pair({ it.inDegrees }, { it.degrees }) + ) + + val pushDownVoltage = + LoggedTunableValue( + "Wrist/pushDownVoltage", + WristConstants.PUSH_DOWN_VOLTAGE, + Pair({ it.inVolts }, { it.volts }) + ) + val idleAngleHasGamepiece = LoggedTunableValue( "Wrist/idleAngleHasGamepiece", @@ -61,6 +79,27 @@ class Wrist(val io: WristIO) : SubsystemBase() { WristConstants.AMP_SCORE_ANGLE, Pair({ it.inDegrees }, { it.degrees }) ) + + val fastAmpAngle = + LoggedTunableValue( + "Wrist/fastAmpAngle", + WristConstants.FAST_AMP_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) + + val fastAmpScoreAngle = + LoggedTunableValue( + "Wrist/ampScoreAngle", + WristConstants.AMP_SCORE_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) + + val passingShotAngle = + LoggedTunableValue( + "Wrist/passingShotAngle", + WristConstants.PASSING_SHOT_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) val subwooferSpeakerShotAngleLow = LoggedTunableValue( "Wrist/subwooferSpeakerShotAngleLow", @@ -121,11 +160,35 @@ class Wrist(val io: WristIO) : SubsystemBase() { "Wrist/kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) ) + private val wristSlot1kP = + LoggedTunableValue("Wrist/slot1kP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })) + private val wristSlot1kI = + LoggedTunableValue( + "Wrist/slot1KI", Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val wristSlot1kD = + LoggedTunableValue( + "Wrist/slot1kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + + private val wristSlot2kP = + LoggedTunableValue("Wrist/slot2kP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })) + private val wristSlot2kI = + LoggedTunableValue( + "Wrist/slot2kI", Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val wristSlot2kD = + LoggedTunableValue( + "Wrist/slot2kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + private val testAngleUp = LoggedTunableValue("Wrist/testAngleUp", Pair({ it.inDegrees }, { it.degrees })) private val testAngleDown = LoggedTunableValue("Wrist/testAngleDown", Pair({ it.inDegrees }, { it.degrees })) + private var wristToleranceRequested: Angle = WristConstants.WRIST_TOLERANCE + var currentRequest: Request.WristRequest = Request.WristRequest.Zero() set(value) { when (value) { @@ -134,6 +197,7 @@ class Wrist(val io: WristIO) : SubsystemBase() { } is Request.WristRequest.TargetingPosition -> { wristPositionTarget = value.wristPosition + wristToleranceRequested = value.wristTolerance } else -> {} } @@ -144,6 +208,8 @@ class Wrist(val io: WristIO) : SubsystemBase() { var isZeroed = false + var travelingUp: Boolean = false + var wristTargetVoltage: ElectricalPotential = 0.0.volts private var lastWristPositionTarget = -1337.0.degrees @@ -176,11 +242,21 @@ class Wrist(val io: WristIO) : SubsystemBase() { } init { + arbitraryFeedforward.initDefault(WristConstants.PID.ARBITRARY_FEEDFORWARD) + if (RobotBase.isReal()) { wristkP.initDefault(WristConstants.PID.REAL_KP) wristkI.initDefault(WristConstants.PID.REAL_KI) wristkD.initDefault(WristConstants.PID.REAL_KD) + wristSlot1kP.initDefault(WristConstants.PID.FIRST_STAGE_KP) + wristSlot1kI.initDefault(WristConstants.PID.FIRST_STAGE_KI) + wristSlot1kD.initDefault(WristConstants.PID.FIRST_STAGE_KD) + + wristSlot2kP.initDefault(WristConstants.PID.SECOND_STAGE_KP) + wristSlot2kI.initDefault(WristConstants.PID.SECOND_STAGE_KI) + wristSlot2kD.initDefault(WristConstants.PID.SECOND_STAGE_KD) + wristkS.initDefault(WristConstants.PID.REAL_WRIST_KS) wristkG.initDefault(WristConstants.PID.REAL_WRIST_KG) wristkV.initDefault(WristConstants.PID.REAL_WRIST_KV) @@ -221,6 +297,15 @@ class Wrist(val io: WristIO) : SubsystemBase() { if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { io.configPID(wristkP.get(), wristkI.get(), wristkD.get()) } + + if (wristSlot1kP.hasChanged() || wristSlot1kI.hasChanged() || wristSlot1kD.hasChanged()) { + io.configPIDSlot1(wristSlot1kP.get(), wristSlot1kI.get(), wristSlot1kD.get()) + } + + if (wristSlot2kP.hasChanged() || wristSlot2kI.hasChanged() || wristSlot2kD.hasChanged()) { + io.configPIDSlot2(wristSlot2kP.get(), wristSlot2kI.get(), wristSlot2kD.get()) + } + if (wristkA.hasChanged() || wristkV.hasChanged() || wristkG.hasChanged() || @@ -248,7 +333,7 @@ class Wrist(val io: WristIO) : SubsystemBase() { nextState = fromRequestToState(currentRequest) } WristStates.ZERO -> { - io.zeroEncoder(-36.25.degrees) + io.zeroEncoder() currentRequest = Request.WristRequest.OpenLoop(0.volts) isZeroed = true nextState = fromRequestToState(currentRequest) @@ -259,9 +344,14 @@ class Wrist(val io: WristIO) : SubsystemBase() { nextState = fromRequestToState(currentRequest) } WristStates.TARGETING_POSITION -> { - Logger.recordOutput("Wrist/RequestedPosition", wristPositionTarget.inDegrees) - - if (wristPositionTarget != lastWristPositionTarget) { + if ((wristPositionTarget - lastWristPositionTarget).absoluteValue < 5.degrees) { + wristProfile = + TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), + wristProfile.initial + ) + } else { val preProfileGenerate = Clock.fpgaTime wristProfile = TrapezoidProfile( @@ -269,13 +359,14 @@ class Wrist(val io: WristIO) : SubsystemBase() { TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), TrapezoidProfile.State(inputs.wristPosition, inputs.wristVelocity) ) - val postProfileGenerate = Clock.fpgaTime Logger.recordOutput( "/Wrist/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds ) + travelingUp = wristPositionTarget > inputs.wristPosition + timeProfileGeneratedAt = Clock.fpgaTime lastWristPositionTarget = wristPositionTarget } @@ -283,6 +374,7 @@ class Wrist(val io: WristIO) : SubsystemBase() { val setPoint: TrapezoidProfile.State = wristProfile.calculate(timeElapsed) setWristPosition(setPoint) Logger.recordOutput("Wrist/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + Logger.recordOutput("Wrist/travelingUp", travelingUp) nextState = fromRequestToState(currentRequest) // if we're transitioning out of targeting position, we want to make sure the next time we // enter targeting position, we regenerate profile (even if the arm setpoint is the same as @@ -309,26 +401,29 @@ class Wrist(val io: WristIO) : SubsystemBase() { if (isOutOfBounds(setPoint.velocity)) { io.setWristVoltage(wristFeedForward.calculate(inputs.wristPosition, 0.degrees.perSecond)) } else { - io.setWristPosition(setPoint.position, feedforward) + if (inputs.wristPosition > 0.5.degrees && travelingUp) { + io.setWristPosition( + setPoint.position, feedforward + arbitraryFeedforward.get(), travelingUp + ) + } else { + io.setWristPosition(setPoint.position, feedforward, travelingUp) + } } - Logger.recordOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) + DebugLogger.recordDebugOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) Logger.recordOutput("Wrist/armFeedForward", feedforward.inVolts) Logger.recordOutput("Wrist/armTargetPosition", setPoint.position.inDegrees) Logger.recordOutput("Wrist/armTargetVelocity", setPoint.velocity.inDegreesPerSecond) } val isAtTargetedPosition: Boolean - get() = true - /* - ( - currentState == WristStates.TARGETING_POSITION && - wristProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && - (inputs.wristPosition - wristPositionTarget).absoluteValue <= - WristConstants.WRIST_TOLERANCE - ) - - */ + get() = + ( + currentState == WristStates.TARGETING_POSITION && + wristProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.wristPosition - wristPositionTarget).absoluteValue <= + wristToleranceRequested + ) || inputs.isSimulated fun setWristVoltage(appliedVoltage: ElectricalPotential) { io.setWristVoltage(appliedVoltage) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt index 20cadc7f..d8f031a0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt @@ -15,6 +15,7 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inRotations import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inDegreesPerSecond @@ -25,6 +26,7 @@ interface WristIO { class WristIOInputs : LoggableInputs { var wristPosition = 0.0.degrees + var wristAbsoluteEncoderPosition = 0.0.degrees var wristVelocity = 0.0.degrees.perSecond var wristAppliedVoltage = 0.0.volts var wristDutyCycle = 0.0.volts @@ -38,7 +40,10 @@ interface WristIO { var isSimulated = false override fun toLog(table: LogTable) { - table.put("wristPostion", wristPosition.inDegrees) + table.put("wristPosition", wristPosition.inDegrees) + table.put("wristAbsoluteEncoderRotations", wristAbsoluteEncoderPosition.inRotations) + table.put("wristPositionRotations", wristPosition.inRotations) + table.put("wristAbosluteEncoderPosition", wristAbsoluteEncoderPosition.inDegrees) table.put("wristVelocity", wristVelocity.inDegreesPerSecond) table.put("wristAppliedVoltage", wristAppliedVoltage.inVolts) table.put("wristStatorCurrent", wristStatorCurrent.inAmperes) @@ -51,6 +56,9 @@ interface WristIO { // wrist logs table.get("wristPostion", wristPosition.inDegrees).let { wristPosition = it.degrees } + table.get("wristPostion", wristAbsoluteEncoderPosition.inDegrees).let { + wristAbsoluteEncoderPosition = it.degrees + } table.get("wristVelocity", wristVelocity.inDegreesPerSecond).let { wristVelocity = it.degrees.perSecond } @@ -82,7 +90,7 @@ interface WristIO { // fun setFeederVoltage (voltage: ElectricalPotential){ // } - fun setWristPosition(position: Angle, feedforward: ElectricalPotential) {} + fun setWristPosition(position: Angle, feedforward: ElectricalPotential, travelingUp: Boolean) {} // fun setRollerBrakeMode (brake: Boolean){ @@ -98,5 +106,17 @@ interface WristIO { kD: DerivativeGain ) {} - fun zeroEncoder(encoderOffset: Angle) {} + fun configPIDSlot1( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + + fun configPIDSlot2( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + + fun zeroEncoder() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt index f22250a8..9d6ff4c5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIONeo.kt @@ -38,7 +38,9 @@ object WristIONeo : WristIO { CANSparkMax(Constants.WRIST.WRIST_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) private val wristSensor = sparkMaxAngularMechanismSensor( - wristSparkMax, WristConstants.WRIST_GEAR_RATIO, WristConstants.WRIST_VOLTAGE_COMPENSATION + wristSparkMax, + WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO, + WristConstants.WRIST_VOLTAGE_COMPENSATION ) private val wristPIDController: SparkMaxPIDController = wristSparkMax.pidController // private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, @@ -54,8 +56,8 @@ object WristIONeo : WristIO { get() { var output = ( - (-throughBoreEncoder.absolutePosition.rotations) * - WristConstants.WRIST_ENCODER_GEAR_RATIO + (-throughBoreEncoder.absolutePosition.rotations) / + WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO ) if (output in (-55).degrees..0.0.degrees) { @@ -68,10 +70,7 @@ object WristIONeo : WristIO { // uses the absolute encoder position to calculate the arm position private val armAbsolutePosition: Angle get() { - return (encoderAbsolutePosition - WristConstants.ABSOLUTE_ENCODER_OFFSET).inDegrees.IEEErem( - 360.0 - ) - .degrees + return (encoderAbsolutePosition).inDegrees.IEEErem(360.0).degrees } init { @@ -138,7 +137,11 @@ object WristIONeo : WristIO { wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) } - override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + override fun setWristPosition( + position: Angle, + feedforward: ElectricalPotential, + travelingUp: Boolean + ) { wristPIDController.setReference( wristSensor.positionToRawUnits( clamp(position, WristConstants.WRIST_MIN_ROTATION, WristConstants.WRIST_MAX_ROTATION) @@ -168,7 +171,7 @@ object WristIONeo : WristIO { } } - override fun zeroEncoder(encoderOffset: Angle) { + override fun zeroEncoder() { wristSparkMax.encoder.position = wristSensor.positionToRawUnits(armAbsolutePosition) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt index ffa20b37..50f9bbc7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt @@ -20,6 +20,7 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inKilogramsMeterSquared import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inVolts @@ -32,7 +33,7 @@ object WristIOSim : WristIO { val wristSim = SingleJointedArmSim( DCMotor.getNEO(1), - 1 / WristConstants.WRIST_GEAR_RATIO, + 1 / WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO, WristConstants.WRIST_INERTIA.inKilogramsMeterSquared, WristConstants.WRIST_LENGTH.inMeters, WristConstants.WRIST_MIN_ROTATION.inRadians, @@ -41,6 +42,8 @@ object WristIOSim : WristIO { 0.0 ) + var wristTargetPosition = -35.0.degrees + init { MotorChecker.add( "Ground Intake", @@ -85,7 +88,7 @@ object WristIOSim : WristIO { override fun updateInputs(inputs: WristIO.WristIOInputs) { wristSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - inputs.wristPosition = wristSim.angleRads.radians + inputs.wristPosition = wristTargetPosition inputs.wristVelocity = wristSim.velocityRadPerSec.radians.perSecond inputs.wristSupplyCurrent = 0.amps inputs.wristAppliedVoltage = appliedVoltage @@ -107,7 +110,12 @@ object WristIOSim : WristIO { appliedVoltage = clampedVoltage } - override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + override fun setWristPosition( + position: Angle, + feedforward: ElectricalPotential, + travelingUp: Boolean + ) { + wristTargetPosition = position val feedback = wristController.calculate(wristSim.angleRads.radians, position) setWristVoltage(feedback + feedforward) } @@ -129,5 +137,5 @@ object WristIOSim : WristIO { } /** recalculates the current position of the neo encoder using value from the absolute encoder */ - override fun zeroEncoder(encoderOffet: Angle) {} + override fun zeroEncoder() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt index 2e08efed..814d739a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt @@ -5,21 +5,27 @@ import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.MagnetSensorConfigs import com.ctre.phoenix6.configs.MotorOutputConfigs import com.ctre.phoenix6.configs.Slot0Configs +import com.ctre.phoenix6.configs.Slot1Configs +import com.ctre.phoenix6.configs.Slot2Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.PositionDutyCycle import com.ctre.phoenix6.controls.VoltageOut import com.ctre.phoenix6.hardware.CANcoder import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.ctre.phoenix6.signals.SensorDirectionValue +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.logging.TunableNumber import com.team4099.lib.phoenix6.PositionVoltage import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes @@ -34,10 +40,12 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inRotations import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond object WristIOTalon : WristIO { @@ -51,10 +59,29 @@ object WristIOTalon : WristIO { var positionRequest = PositionVoltage(-1337.degrees, slot = 0, feedforward = -1337.volts) + val drivenPulley = TunableNumber("Wrist/drivenPulley", 50.0) + val drivingPulley = TunableNumber("Wrist/drivingPulley", 32.0) + + val firstStagePosErrSwitchThreshold = + LoggedTunableValue("Wrist/slot1SwitchThreshold", Pair({ it.inDegrees }, { it.degrees })) + val secondStagePosErrSwitchThreshold = + LoggedTunableValue("Wrist/slot2SwitchThreshold", Pair({ it.inDegrees }, { it.degrees })) + + val firstStageVelocitySwitchThreshold = + LoggedTunableValue( + "Wrist/slot1VelSwitchThreshold", + Pair({ it.inDegreesPerSecond }, { it.degrees.perSecond }) + ) + val secondStageVelocitySwitchThreshold = + LoggedTunableValue( + "Wrist/slot2VelSwitchThreshold", + Pair({ it.inDegreesPerSecond }, { it.degrees.perSecond }) + ) + private val wristSensor = ctreAngularMechanismSensor( wristTalon, - WristConstants.WRIST_GEAR_RATIO, + 1.0, WristConstants.VOLTAGE_COMPENSATION, ) @@ -65,28 +92,41 @@ object WristIOTalon : WristIO { var motorVoltage: StatusSignal var motorTorque: StatusSignal var motorAcceleration: StatusSignal + var absoluteEncoderSignal: StatusSignal + + var angleToZero: Angle = 0.0.degrees init { + firstStagePosErrSwitchThreshold.initDefault(WristConstants.PID.FIRST_STAGE_POS_SWITCH_THRESHOLD) + secondStagePosErrSwitchThreshold.initDefault( + WristConstants.PID.SECOND_STAGE_POS_SWITCH_THRESHOLD + ) + + firstStageVelocitySwitchThreshold.initDefault( + WristConstants.PID.FIRST_STAGE_VEL_SWITCH_THRESHOLD + ) + secondStageVelocitySwitchThreshold.initDefault( + WristConstants.PID.SECOND_STAGE_VEL_SWITCH_THRESHOLD + ) + wristTalon.configurator.apply(TalonFXConfiguration()) wristTalon.clearStickyFaults() absoluteEncoderConfiguration.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf - absoluteEncoderConfiguration.SensorDirection = SensorDirectionValue.CounterClockwise_Positive - absoluteEncoderConfiguration.MagnetOffset = - WristConstants.ABSOLUTE_ENCODER_OFFSET.inDegrees / 180 + absoluteEncoderConfiguration.SensorDirection = SensorDirectionValue.Clockwise_Positive + absoluteEncoderConfiguration.MagnetOffset = WristConstants.ABSOLUTE_ENCODER_OFFSET.inRotations - /* absoluteEncoder.configurator.apply(absoluteEncoderConfiguration) wristConfiguration.Feedback.FeedbackRemoteSensorID = absoluteEncoder.deviceID - wristConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder - - wristConfiguration.Feedback.SensorToMechanismRatio = WristConstants.WRIST_GEAR_RATIO - wristConfiguration.Feedback.RotorToSensorRatio = WristConstants.WRIST_ENCODER_GEAR_RATIO + wristConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder + wristConfiguration.Feedback.SensorToMechanismRatio = + WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO + wristConfiguration.Feedback.RotorToSensorRatio = + WristConstants.MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO - */ wristConfiguration.Slot0.kP = wristSensor.proportionalPositionGainToRawUnits(WristConstants.PID.REAL_KP) wristConfiguration.Slot0.kI = @@ -94,6 +134,20 @@ object WristIOTalon : WristIO { wristConfiguration.Slot0.kD = wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.REAL_KD) + wristConfiguration.Slot1.kP = + wristSensor.proportionalPositionGainToRawUnits(WristConstants.PID.FIRST_STAGE_KP) + wristConfiguration.Slot1.kI = + wristSensor.integralPositionGainToRawUnits(WristConstants.PID.FIRST_STAGE_KI) + wristConfiguration.Slot1.kD = + wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.FIRST_STAGE_KD) + + wristConfiguration.Slot2.kP = + wristSensor.proportionalPositionGainToRawUnits(WristConstants.PID.SECOND_STAGE_KP) + wristConfiguration.Slot2.kI = + wristSensor.integralPositionGainToRawUnits(WristConstants.PID.SECOND_STAGE_KI) + wristConfiguration.Slot2.kD = + wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.SECOND_STAGE_KD) + wristConfiguration.CurrentLimits.SupplyCurrentLimit = WristConstants.WRIST_SUPPLY_CURRENT_LIMIT.inAmperes wristConfiguration.CurrentLimits.SupplyCurrentThreshold = @@ -106,7 +160,7 @@ object WristIOTalon : WristIO { wristConfiguration.CurrentLimits.StatorCurrentLimitEnable = false wristConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - wristConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + wristConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive wristTalon.configurator.apply(wristConfiguration) @@ -118,6 +172,8 @@ object WristIOTalon : WristIO { motorTorque = wristTalon.torqueCurrent motorAcceleration = wristTalon.acceleration + absoluteEncoderSignal = absoluteEncoder.position + MotorChecker.add( "Wrist", "Wrist", @@ -131,6 +187,32 @@ object WristIOTalon : WristIO { ) } + override fun configPIDSlot2( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + val PIDConfig = Slot2Configs() + PIDConfig.kP = wristSensor.proportionalPositionGainToRawUnits(kP) + PIDConfig.kI = wristSensor.integralPositionGainToRawUnits(kI) + PIDConfig.kD = wristSensor.derivativePositionGainToRawUnits(kD) + + wristTalon.configurator.apply(PIDConfig) + } + + override fun configPIDSlot1( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + val PIDConfig = Slot1Configs() + PIDConfig.kP = wristSensor.proportionalPositionGainToRawUnits(kP) + PIDConfig.kI = wristSensor.integralPositionGainToRawUnits(kI) + PIDConfig.kD = wristSensor.derivativePositionGainToRawUnits(kD) + + wristTalon.configurator.apply(PIDConfig) + } + override fun configPID( kP: ProportionalGain, kI: IntegralGain, @@ -148,16 +230,35 @@ object WristIOTalon : WristIO { wristTalon.setControl(VoltageOut(voltage.inVolts)) } - override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + override fun setWristPosition( + position: Angle, + feedforward: ElectricalPotential, + travelingUp: Boolean + ) { positionRequest.setFeedforward(feedforward) positionRequest.setPosition(position) + + val curError = (wristSensor.position - position) + val curVel = wristSensor.velocity + + var slot = if (travelingUp) 0 else 1 + + if (curError.absoluteValue <= secondStagePosErrSwitchThreshold.get() && + curVel.absoluteValue <= secondStageVelocitySwitchThreshold.get() + ) { + slot = 2 + } + + Logger.recordOutput("Wrist/feedForwardApplied", feedforward.inVolts) + Logger.recordOutput("Wrist/slotBeingUsed", slot) + wristTalon.setControl( PositionDutyCycle( wristSensor.positionToRawUnits(position), wristSensor.velocityToRawUnits(0.0.radians.perSecond), true, feedforward.inVolts, - 0, + slot, false, false, false @@ -167,17 +268,54 @@ object WristIOTalon : WristIO { private fun updateSignals() { BaseStatusSignal.refreshAll( - motorTorque, motorVoltage, dutyCycle, supplyCurrentSignal, tempSignal, motorAcceleration + motorTorque, + motorVoltage, + dutyCycle, + supplyCurrentSignal, + statorCurrentSignal, + tempSignal, + motorAcceleration, + absoluteEncoderSignal ) } override fun updateInputs(inputs: WristIO.WristIOInputs) { - updateSignals() + wristTalon.rotorPosition.refresh() + wristTalon.position.refresh() + Logger.recordOutput( + "Wrist/rotorTMechanismDegrees", + ( + ( + wristTalon.rotorPosition.value.rotations / + WristConstants.MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO / + (1.06488 / 1.0) + ) + angleToZero + ) + .inDegrees + ) + Logger.recordOutput("Wrist/absoluteEncoderRots", absoluteEncoderSignal.value) + Logger.recordOutput( + "Wrist/absoluteEncoderTMechanismDegrees", + (absoluteEncoderSignal.value / (drivenPulley.get() / drivingPulley.get())) + .rotations + .inDegrees + ) + Logger.recordOutput("Wrist/hopefullyAbsoluteEncoderRots", wristTalon.position.value) + + inputs.wristAbsoluteEncoderPosition = + (absoluteEncoderSignal.value).rotations / + WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO inputs.wristPosition = wristSensor.position + // wristTalon.position.value.rotations / + // WristConstants.MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO / + // WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO + angleToZero inputs.wristAcceleration = - (motorAcceleration.value * WristConstants.WRIST_GEAR_RATIO).rotations.perSecond.perSecond + (motorAcceleration.value * WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO) + .rotations + .perSecond + .perSecond inputs.wristVelocity = wristSensor.velocity // TODO fix unit for torque inputs.wristTorque = motorTorque.value @@ -188,7 +326,7 @@ object WristIOTalon : WristIO { inputs.wristTemperature = tempSignal.value.celsius if (inputs.wristPosition < WristConstants.WRIST_MIN_ROTATION) { - wristTalon.setPosition(wristSensor.positionToRawUnits(WristConstants.WRIST_MIN_ROTATION)) + zeroEncoder() } } @@ -203,7 +341,11 @@ object WristIOTalon : WristIO { wristTalon.configurator.apply(motorOutputConfig) } - override fun zeroEncoder(encoderAngle: Angle) { - wristTalon.setPosition(wristSensor.positionToRawUnits(encoderAngle)) + override fun zeroEncoder() { + angleToZero = + (absoluteEncoderSignal.value).rotations / + WristConstants.ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO + Logger.recordOutput("Wrist/angleToZero", angleToZero.inDegrees) + wristTalon.setPosition(angleToZero.inRotations) } } diff --git a/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt b/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt new file mode 100644 index 00000000..35e41368 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt @@ -0,0 +1,127 @@ +package com.team4099.robot2023.util + +import com.team4099.robot2023.config.constants.Constants +import edu.wpi.first.units.Measure +import edu.wpi.first.util.WPISerializable +import edu.wpi.first.util.struct.Struct +import edu.wpi.first.util.struct.StructSerializable +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d +import org.littletonrobotics.junction.Logger + +class DebugLogger { + companion object { + inline fun > recordDebugOutput(key: String, value: E) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: T) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun > recordDebugOutput(key: String, value: Measure) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Struct) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, vararg value: T) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, *value) + } + } + + inline fun recordDebugOutput(key: String, value: Mechanism2d) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Array) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: BooleanArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Boolean) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: ByteArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: DoubleArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Double) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Float) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: FloatArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Int) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: IntArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: Long) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: LongArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + + inline fun recordDebugOutput(key: String, value: String) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/util/Extensions.kt b/src/main/kotlin/com/team4099/robot2023/util/Extensions.kt new file mode 100644 index 00000000..1e19cfef --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/util/Extensions.kt @@ -0,0 +1,126 @@ +package com.team4099.robot2023.util + +import com.team4099.robot2023.config.constants.Constants +import edu.wpi.first.units.Measure +import edu.wpi.first.util.WPISerializable +import edu.wpi.first.util.struct.Struct +import edu.wpi.first.util.struct.StructSerializable +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d +import org.littletonrobotics.junction.Logger + +inline fun > Logger.recordDebugOutput(key: String, value: E) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: T) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun > Logger.recordDebugOutput( + key: String, + value: Measure +) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Struct) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, vararg value: T) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, *value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Mechanism2d) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Array) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: BooleanArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Boolean) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: ByteArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: DoubleArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Double) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Float) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: FloatArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Int) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: IntArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: Long) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: LongArray) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} + +inline fun Logger.recordDebugOutput(key: String, value: String) { + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput(key, value) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt index a6697f37..c5175548 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.util import com.team4099.lib.hal.Clock import com.team4099.lib.math.asTransform2d +import com.team4099.lib.vision.TimestampedTrigVisionUpdate import com.team4099.lib.vision.TimestampedVisionUpdate import edu.wpi.first.math.Matrix import edu.wpi.first.math.Nat @@ -37,6 +38,9 @@ class FieldFrameEstimator(stateStdDevs: Matrix) { private var odometryTField: Transform2d = Transform2d(Translation2d(0.meters, 0.meters), 0.radians) + private var odometryTSpeaker: Transform2d = + Transform2d(Translation2d(0.meters, 0.meters), 0.radians) + private val updates: NavigableMap = TreeMap() private val q: Matrix = Matrix(Nat.N3(), Nat.N1()) @@ -45,6 +49,11 @@ class FieldFrameEstimator(stateStdDevs: Matrix) { return odometryTField } + /** Returns the latest speaker pose in the odometry frame. */ + fun getLatestOdometryTSpeaker(): Transform2d { + return odometryTSpeaker + } + /** Resets the field frame transform to a known pose. */ fun resetFieldFrameFilter(transform: Transform2d) { baseOdometryTField = transform @@ -58,6 +67,36 @@ class FieldFrameEstimator(stateStdDevs: Matrix) { update() } + fun addSpeakerVisionData(visionData: TimestampedTrigVisionUpdate) { + val timestamp: Time = visionData.timestamp + if (updates.containsKey(timestamp)) { + // There was already an odometry update at this timestamp, add to it + val odomTRobotAtVisionTimestamp = updates[timestamp]!!.odomTRobot + val robotTSpeaker = visionData.robotTSpeaker + odometryTSpeaker = odomTRobotAtVisionTimestamp.transformBy(robotTSpeaker).asTransform2d() + } else { + // Insert a new update + val prevUpdate = updates.floorEntry(timestamp) + val nextUpdate = updates.ceilingEntry(timestamp) + if (prevUpdate == null || nextUpdate == null) { + // Outside the range of existing data + return + } + + // Create partial twists (prev -> vision, vision -> next) + val prevToVisionTwist = + multiplyTwist( + prevUpdate.value.odomTRobot.log(nextUpdate.value.odomTRobot), + (timestamp - prevUpdate.key) / (nextUpdate.key - prevUpdate.key) + ) + + // Add new pose updates + val odomTRobotAtVisionTimestamp = prevUpdate.value.odomTRobot.exp(prevToVisionTwist) + val robotTSpeaker = visionData.robotTSpeaker + odometryTSpeaker = odomTRobotAtVisionTimestamp.transformBy(robotTSpeaker).asTransform2d() + } + } + /** Records a new set of vision updates. */ fun addVisionData(visionData: List) { for (timestampedVisionUpdate in visionData) { diff --git a/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt b/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt index 388dc94e..3851db03 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt @@ -1,24 +1,30 @@ package com.team4099.robot2023.util -import com.team4099.utils.LimelightHelpers.LimelightTarget_Retro +import com.team4099.utils.LimelightHelpers.LimelightTarget_Detector +import org.team4099.lib.units.base.Decimal +import org.team4099.lib.units.base.percent import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees data class LimelightReading( + val className: String, + val confidence: Decimal, val tx: Angle, val ty: Angle, val txPixel: Double, val tyPixel: Double, - val ts: Angle + val ta: Decimal ) { constructor( - limelightReading: LimelightTarget_Retro + limelightReading: LimelightTarget_Detector ) : this( + limelightReading.className, + limelightReading.confidence.percent, limelightReading.tx.degrees, limelightReading.ty.degrees, limelightReading.tx_pixels, limelightReading.ty_pixels, - limelightReading.ts.degrees - ) {} + limelightReading.ta.percent + ) } diff --git a/src/main/kotlin/com/team4099/robot2023/util/NTSafePublisher.kt b/src/main/kotlin/com/team4099/robot2023/util/NTSafePublisher.kt index 7d47ed3b..066efc50 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/NTSafePublisher.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/NTSafePublisher.kt @@ -43,9 +43,7 @@ class NTSafePublisher : LogDataReceiver { var publisher = publishers[key] if (publisher == null) { publisher = - rootTable - .getTopic(key) - .genericPublish(newValue.type.nT4Type, PubSubOption.sendAll(true)) + rootTable.getTopic(key).genericPublish(newValue.nT4Type, PubSubOption.sendAll(true)) publishers[key] = publisher } diff --git a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt index d6cd821f..d1ce504c 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt @@ -110,14 +110,14 @@ class NoteSimulation( var flywheelAngularVelocitySupplier: () -> AngularVelocity = { 0.0.rotations.perMinute } fun periodic() { - Logger.recordOutput("NoteData/$id/inclinationAngle", inclinationAngleTheta.inDegrees) - Logger.recordOutput("NoteData/$id/azimuthalAngle", azimuthalAnglePhi.inDegrees) - Logger.recordOutput( + DebugLogger.recordDebugOutput("NoteData/$id/inclinationAngle", inclinationAngleTheta.inDegrees) + DebugLogger.recordDebugOutput("NoteData/$id/azimuthalAngle", azimuthalAnglePhi.inDegrees) + DebugLogger.recordDebugOutput( "NoteData/$id/launchVelocityMetersPerSecond", launchVelocity.inMetersPerSecond ) - Logger.recordOutput("NoteData/$id/startPose", launchStartPose.pose3d) - Logger.recordOutput("NoteData/$id/state", currentState.name) - Logger.recordOutput( + DebugLogger.recordDebugOutput("NoteData/$id/startPose", launchStartPose.pose3d) + DebugLogger.recordDebugOutput("NoteData/$id/state", currentState.name) + DebugLogger.recordDebugOutput( "NoteSimulation/IntakePose", poseSupplier().transformBy(IntakeConstants.INTAKE_TRANSFORM).pose2d ) diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index bdbd93d2..d15b924f 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.2", + "version": "3.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.2" + "version": "3.1.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.2" + "version": "3.1.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.2" + "version": "3.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.2", + "version": "3.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ @@ -39,4 +39,4 @@ } ], "cppDependencies": [] -} \ No newline at end of file +} diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 00000000..143bd318 --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix6.json similarity index 100% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix6.json