diff --git a/src/main/deploy/pathplanner/BlueBump2.path b/src/main/deploy/pathplanner/BlueBump2.path index 647d259..288edeb 100644 --- a/src/main/deploy/pathplanner/BlueBump2.path +++ b/src/main/deploy/pathplanner/BlueBump2.path @@ -10,7 +10,7 @@ "x": 2.9248226748842034, "y": 0.707445764015732 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -19,13 +19,39 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } }, + { + "anchorPoint": { + "x": 4.553732702329884, + "y": 0.7547402256002796 + }, + "prevControl": { + "x": 4.00655431668968, + "y": 0.7547402256002796 + }, + "nextControl": { + "x": 5.698764143187168, + "y": 0.7547402256002796 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, { "anchorPoint": { "x": 7.084232792072272, @@ -45,7 +71,9 @@ "isLocked": false, "isStopPoint": true, "stopEvent": { - "names": [], + "names": [ + "intake" + ], "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 @@ -78,15 +106,15 @@ ], "markers": [ { - "position": 0.6482954545454549, + "position": 2.180397727272727, "names": [ - "intakeCube" + "stow" ] }, { - "position": 1.180397727272727, + "position": 1.461818181818112, "names": [ - "stow" + "intakeCube" ] } ] diff --git a/src/main/deploy/pathplanner/BlueBump3.path b/src/main/deploy/pathplanner/BlueBump3.path index 3968a67..839e38c 100644 --- a/src/main/deploy/pathplanner/BlueBump3.path +++ b/src/main/deploy/pathplanner/BlueBump3.path @@ -7,10 +7,10 @@ }, "prevControl": null, "nextControl": { - "x": 2.5561411144717194, - "y": 0.8222286896405007 + "x": 2.5636022660440645, + "y": 0.9234490801784156 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -19,13 +19,39 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } }, + { + "anchorPoint": { + "x": 2.952768587420199, + "y": 0.9262720950548883 + }, + "prevControl": { + "x": 2.549424522428349, + "y": 0.9262720950548883 + }, + "nextControl": { + "x": 3.3561126524120537, + "y": 0.9262720950548883 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, { "anchorPoint": { "x": 7.084232792072272, @@ -45,7 +71,9 @@ "isLocked": false, "isStopPoint": true, "stopEvent": { - "names": [], + "names": [ + "intake" + ], "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 @@ -80,16 +108,16 @@ }, { "anchorPoint": { - "x": 5.137659971279337, - "y": 0.8471099580535155 + "x": 5.171247432366477, + "y": 0.8805302632003253 }, "prevControl": { - "x": 2.9543595016493014, - "y": 0.8471099580535155 + "x": 2.9880787766931345, + "y": 0.8565393988522682 }, "nextControl": { - "x": 6.317415584492585, - "y": 0.8471099580535155 + "x": 6.211874107057772, + "y": 0.8919657211639652 }, "holonomicAngle": 180.0, "isReversal": false, @@ -107,49 +135,26 @@ }, { "anchorPoint": { - "x": 5.966520341679218, - "y": 2.2581620919611276 - }, - "prevControl": { - "x": 5.587750523954341, - "y": 2.2581620919611276 - }, - "nextControl": { - "x": 6.530263077308415, - "y": 2.2581620919611276 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.0535176390814565, - "y": 2.279802313386474 + "x": 7.252500781749067, + "y": 2.184172471055355 }, "prevControl": { - "x": 6.279370735426079, - "y": 2.279802313386474 + "x": 6.646421509676115, + "y": 1.3493840397095893 }, "nextControl": { - "x": 6.279370735426079, - "y": 2.279802313386474 + "x": 6.646421509676115, + "y": 1.3493840397095893 }, - "holonomicAngle": 180.0, + "holonomicAngle": -135.0, "isReversal": true, "velOverride": null, "isLocked": false, "isStopPoint": true, "stopEvent": { - "names": [], + "names": [ + "intake" + ], "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 @@ -161,12 +166,12 @@ "y": 0.9518705691471896 }, "prevControl": { - "x": 6.105536326319467, - "y": 0.7280388779527571 + "x": 5.777326704432687, + "y": 1.0177557587707124 }, "nextControl": { - "x": 3.157000450684807, - "y": 1.5189547755161845 + "x": 3.10056940291484, + "y": 0.6693828681452603 }, "holonomicAngle": -179.84827369590738, "isReversal": false, @@ -207,25 +212,25 @@ ], "markers": [ { - "position": 0.6482954545454549, + "position": 2.180397727272727, "names": [ - "intakeCube" + "stow" ] }, { - "position": 1.180397727272727, + "position": 5.426491477272728, "names": [ "stow" ] }, { - "position": 5.426491477272728, + "position": 4.094637784090915, "names": [ - "stow" + "intakeCube" ] }, { - "position": 3.0946377840909154, + "position": 1.8036363636362602, "names": [ "intakeCube" ] diff --git a/src/main/deploy/pathplanner/BlueFlat2.path b/src/main/deploy/pathplanner/BlueFlat2.path index 91b7da4..6f216ea 100644 --- a/src/main/deploy/pathplanner/BlueFlat2.path +++ b/src/main/deploy/pathplanner/BlueFlat2.path @@ -10,7 +10,7 @@ "x": 2.33799056266904, "y": 4.53447404705319 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreMid" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -28,16 +29,41 @@ }, { "anchorPoint": { - "x": 6.844744350814339, - "y": 4.588474876093861 + "x": 4.885360983275461, + "y": 4.53 }, "prevControl": { - "x": 6.377112164852355, - "y": 4.588474876093861 + "x": 4.596970165887906, + "y": 4.53 }, "nextControl": { - "x": 6.377112164852355, - "y": 4.588474876093861 + "x": 5.173751800663019, + "y": 4.53 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.93230795876713, + "y": 4.597054101383524 + }, + "prevControl": { + "x": 6.464675772805147, + "y": 4.597054101383524 + }, + "nextControl": { + "x": 6.464675772805147, + "y": 4.597054101383524 }, "holonomicAngle": 180.0, "isReversal": true, @@ -69,8 +95,10 @@ "isLocked": false, "isStopPoint": false, "stopEvent": { - "names": [], - "executionBehavior": "parallel", + "names": [ + "scoreLow" + ], + "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } @@ -78,15 +106,15 @@ ], "markers": [ { - "position": 0.0, + "position": 2.439999999999932, "names": [ - "event" + "stow" ] }, { - "position": 0.0, + "position": 1.461818181818124, "names": [ - "event" + "intakeCube" ] } ] diff --git a/src/main/deploy/pathplanner/BlueFlat3.path b/src/main/deploy/pathplanner/BlueFlat3.path index 9715174..312d1f6 100644 --- a/src/main/deploy/pathplanner/BlueFlat3.path +++ b/src/main/deploy/pathplanner/BlueFlat3.path @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -73,7 +74,8 @@ "isStopPoint": true, "stopEvent": { "names": [ - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -176,7 +178,8 @@ "isStopPoint": false, "stopEvent": { "names": [ - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -186,15 +189,27 @@ ], "markers": [ { - "position": 0.0, + "position": 0.6981818181817786, "names": [ - "event" + "intakeCube" ] }, { - "position": 0.0, + "position": 1.352727272727201, "names": [ - "event" + "stow" + ] + }, + { + "position": 2.967272727272592, + "names": [ + "intakeCube" + ] + }, + { + "position": 4.887272727272499, + "names": [ + "stow" ] } ] diff --git a/src/main/deploy/pathplanner/BlueMid1.path b/src/main/deploy/pathplanner/BlueMid1.path index 13efc02..560d2dd 100644 --- a/src/main/deploy/pathplanner/BlueMid1.path +++ b/src/main/deploy/pathplanner/BlueMid1.path @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", diff --git a/src/main/deploy/pathplanner/BlueMid1Balance.path b/src/main/deploy/pathplanner/BlueMid1Balance.path index 2f27882..9e46c79 100644 --- a/src/main/deploy/pathplanner/BlueMid1Balance.path +++ b/src/main/deploy/pathplanner/BlueMid1Balance.path @@ -10,7 +10,7 @@ "x": 2.6029238915962654, "y": 2.8643188824349104 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -39,7 +40,7 @@ "x": 6.598881907977464, "y": 2.86 }, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -63,7 +64,7 @@ "y": 2.86 }, "nextControl": null, - "holonomicAngle": 180.0, + "holonomicAngle": 0.0, "isReversal": false, "velOverride": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/RedBump2.path b/src/main/deploy/pathplanner/RedBump2.path index 0f7d204..9107e9c 100644 --- a/src/main/deploy/pathplanner/RedBump2.path +++ b/src/main/deploy/pathplanner/RedBump2.path @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -70,7 +71,8 @@ "isStopPoint": false, "stopEvent": { "names": [ - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -78,5 +80,18 @@ } } ], - "markers": [] + "markers": [ + { + "position": 0.6690909090908629, + "names": [ + "intakeCube" + ] + }, + { + "position": 1.3963636363635585, + "names": [ + "stow" + ] + } + ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/RedBump3.path b/src/main/deploy/pathplanner/RedBump3.path index 68b81aa..326523f 100644 --- a/src/main/deploy/pathplanner/RedBump3.path +++ b/src/main/deploy/pathplanner/RedBump3.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 14.705770806221206, - "y": 0.9476744751204378 + "x": 14.666722554699088, + "y": 0.9234490801784156 }, "prevControl": null, "nextControl": { - "x": 13.7702550587964, - "y": 0.9604392140257116 + "x": 13.976397733955935, + "y": 0.9234490801784156 }, "holonomicAngle": 180, "isReversal": false, @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreMid" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -28,18 +29,43 @@ }, { "anchorPoint": { - "x": 9.922477974151807, - "y": 0.9139752505221581 + "x": 13.5872314125798, + "y": 0.9262720950548883 }, "prevControl": { - "x": 10.994965353501566, - "y": 0.7418507243463536 + "x": 13.99057547757165, + "y": 0.9262720950548883 }, "nextControl": { - "x": 10.994965353501566, - "y": 0.7418507243463536 + "x": 13.183887347587945, + "y": 0.9262720950548883 }, - "holonomicAngle": 180, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.455767207927728, + "y": 0.7442939751524409 + }, + "prevControl": { + "x": 13.129299764180232, + "y": 0.7442939751524409 + }, + "nextControl": { + "x": 13.129299764180232, + "y": 0.7442939751524409 + }, + "holonomicAngle": 0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -55,19 +81,19 @@ }, { "anchorPoint": { - "x": 14.705870530743905, - "y": 0.9414716098086577 + "x": 14.255309036310747, + "y": 1.9355532610348662 }, "prevControl": { - "x": 15.298223013277966, - "y": 0.9290171734939764 + "x": 14.318883419530373, + "y": 0.5129330824943502 }, "nextControl": { - "x": 10.584156283135474, - "y": 1.0281322200327447 + "x": 14.318883419530373, + "y": 0.5129330824943502 }, - "holonomicAngle": 180, - "isReversal": false, + "holonomicAngle": 0, + "isReversal": true, "velOverride": null, "isLocked": false, "isStopPoint": true, @@ -82,18 +108,45 @@ }, { "anchorPoint": { - "x": 9.939281556226328, - "y": 2.1799780661663237 + "x": 11.368752567633521, + "y": 0.8805302632003253 }, "prevControl": { - "x": 11.084966735236017, - "y": 0.29353913255995345 + "x": 13.551921223306865, + "y": 0.8565393988522682 }, "nextControl": { - "x": 11.084966735236017, - "y": 0.29353913255995345 + "x": 10.328125892942227, + "y": 0.8919657211639652 }, - "holonomicAngle": 180.38919096971816, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "intakeCube" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.287499218250932, + "y": 2.184172471055355 + }, + "prevControl": { + "x": 9.893578490323884, + "y": 1.3493840397095893 + }, + "nextControl": { + "x": 9.893578490323884, + "y": 1.3493840397095893 + }, + "holonomicAngle": 315, "isReversal": true, "velOverride": null, "isLocked": false, @@ -109,18 +162,18 @@ }, { "anchorPoint": { - "x": 11.965284959347784, - "y": 0.841276073475714 + "x": 11.26890861735147, + "y": 0.9518705691471896 }, "prevControl": { - "x": 11.036136181050997, - "y": 0.866273801480432 + "x": 10.762673295567312, + "y": 1.0177557587707124 }, "nextControl": { - "x": 13.125480056409952, - "y": 0.8100622978714117 + "x": 13.43943059708516, + "y": 0.6693828681452603 }, - "holonomicAngle": 178.90205154311627, + "holonomicAngle": 359.8482736959074, "isReversal": false, "velOverride": null, "isLocked": false, @@ -134,22 +187,22 @@ }, { "anchorPoint": { - "x": 14.695698629428765, - "y": 1.883529285922417 + "x": 14.381759731091117, + "y": 1.3286298158981675 }, "prevControl": { - "x": 14.889513239291265, - "y": 0.6492887307573895 + "x": 14.230826665988522, + "y": 1.1695193399343808 }, "nextControl": null, - "holonomicAngle": 180, + "holonomicAngle": 339.40381737320433, "isReversal": false, "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { "names": [ - "scoreMid" + "scoreLow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -157,5 +210,30 @@ } } ], - "markers": [] + "markers": [ + { + "position": 2.180397727272727, + "names": [ + "stow" + ] + }, + { + "position": 5.426491477272728, + "names": [ + "stow" + ] + }, + { + "position": 4.094637784090915, + "names": [ + "intakeCube" + ] + }, + { + "position": 1.8036363636362602, + "names": [ + "intakeCube" + ] + } + ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/RedFlat2.path b/src/main/deploy/pathplanner/RedFlat2.path index 6a54739..c664b56 100644 --- a/src/main/deploy/pathplanner/RedFlat2.path +++ b/src/main/deploy/pathplanner/RedFlat2.path @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -28,16 +29,16 @@ }, { "anchorPoint": { - "x": 9.754506474080623, - "y": 4.550679281991526 + "x": 9.608205122259033, + "y": 4.562747727492601 }, "prevControl": { - "x": 10.22337445770565, - "y": 4.550679281991526 + "x": 10.07707310588406, + "y": 4.562747727492601 }, "nextControl": { - "x": 10.22337445770565, - "y": 4.550679281991526 + "x": 10.07707310588406, + "y": 4.562747727492601 }, "holonomicAngle": 0.0, "isReversal": true, @@ -70,7 +71,8 @@ "isStopPoint": false, "stopEvent": { "names": [ - "score" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -78,5 +80,18 @@ } } ], - "markers": [] + "markers": [ + { + "position": 0.5963636363636233, + "names": [ + "intakeCube" + ] + }, + { + "position": 1.381818181818111, + "names": [ + "stow" + ] + } + ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/RedMid1.path b/src/main/deploy/pathplanner/RedMid1.path index bdb290b..17b33d6 100644 --- a/src/main/deploy/pathplanner/RedMid1.path +++ b/src/main/deploy/pathplanner/RedMid1.path @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreLow" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", diff --git a/src/main/deploy/pathplanner/RedMid1Balance.path b/src/main/deploy/pathplanner/RedMid1Balance.path index 5fc069c..559f1dd 100644 --- a/src/main/deploy/pathplanner/RedMid1Balance.path +++ b/src/main/deploy/pathplanner/RedMid1Balance.path @@ -19,7 +19,8 @@ "names": [ "home", "preload", - "scoreMid" + "scoreHigh", + "stow" ], "executionBehavior": "sequential", "waitBehavior": "none", @@ -28,37 +29,39 @@ }, { "anchorPoint": { - "x": 11.072540122053926, - "y": 2.79 + "x": 9.608205122259033, + "y": 2.86 }, "prevControl": { - "x": 11.564360689269272, - "y": 2.79 + "x": 10.100025689474379, + "y": 2.86 }, "nextControl": { - "x": 11.564360689269272, - "y": 2.79 + "x": 10.100025689474379, + "y": 2.86 }, "holonomicAngle": 0.0, "isReversal": true, "velOverride": null, "isLocked": false, - "isStopPoint": false, + "isStopPoint": true, "stopEvent": { - "names": [], - "executionBehavior": "parallel", + "names": [ + "stop" + ], + "executionBehavior": "sequential", "waitBehavior": "none", "waitTime": 0 } }, { "anchorPoint": { - "x": 12.571293134512105, - "y": 2.79 + "x": 12.169747706114528, + "y": 2.86 }, "prevControl": { - "x": 11.795264326526624, - "y": 2.79 + "x": 11.393718898129046, + "y": 2.86 }, "nextControl": null, "holonomicAngle": 0, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 790cf04..60dde89 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,8 +11,8 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -131,7 +131,6 @@ public Robot() { // This must be run before any commands are scheduled LifecycleSubsystemManager.getInstance().ready(); - configureButtonBindings(); enableLiveWindowInTest(false); @@ -155,16 +154,15 @@ private void configureButtonBindings() { .onTrue( intake .setStateCommand(IntakeState.INTAKING) - .alongWith(wrist.goToAngle(Positions.INTAKING) - .alongWith(Commands.print("hghgh"))) + .alongWith(wrist.goToAngle(Positions.INTAKING).alongWith(Commands.print("hghgh"))) .until(() -> intake.hasCube()) .andThen(wrist.goToAngle(Positions.STOWED).withName("TeleopIntakeCommand"))) - //.alongWith(intake.setStateCommand(IntakeState.STOPPED)))) + // .alongWith(intake.setStateCommand(IntakeState.STOPPED)))) .onFalse( wrist .goToAngle(Positions.STOWED) .alongWith(intake.setStateCommand(IntakeState.STOPPED))); - //.alongWith(Commands.runOnce(() -> intake.setHasCube(true)))); + // .alongWith(Commands.runOnce(() -> intake.setHasCube(true)))); // outtake driveController .rightTrigger(0.5) @@ -223,6 +221,34 @@ private void configureButtonBindings() { wrist .goToAngle(Positions.STOWED) .alongWith(intake.setStateCommand(IntakeState.STOPPED))); + + operatorController + .povUp() + .onTrue( + wrist + .goToAngle(Positions.SHOOT_ON_HIGH_FOLLOW_THRU) + .alongWith( + Commands.waitUntil(() -> wrist.pastAngle(Positions.SHOOT_ON_HIGH)) + .andThen(intake.setStateCommand(IntakeState.SHOOTING)))) + .onFalse( + wrist + .goToAngle(Positions.STOWED) + .alongWith(intake.setStateCommand(IntakeState.STOPPED))); + + operatorController + .povDown() + .onTrue( + wrist + .goToAngle(Positions.SHOOT_ON_MID_FOLLOW_THRU) + .alongWith( + Commands.waitUntil(() -> wrist.pastAngle(Positions.SHOOT_ON_MID)) + .andThen(intake.setStateCommand(IntakeState.SHOOTING)))) + .onFalse( + wrist + .goToAngle(Positions.STOWED) + .alongWith(intake.setStateCommand(IntakeState.STOPPED))); + + } @Override @@ -231,7 +257,8 @@ public void robotPeriodic() { LifecycleSubsystemManager.getInstance().log(); - Logger.getInstance().recordOutput("Intake/requestingIntake", driveController.leftTrigger(0.5).getAsBoolean()); + Logger.getInstance() + .recordOutput("Intake/requestingIntake", driveController.leftTrigger(0.5).getAsBoolean()); } @Override diff --git a/src/main/java/frc/robot/autos/Autos.java b/src/main/java/frc/robot/autos/Autos.java index e24c97a..a4c9b1d 100644 --- a/src/main/java/frc/robot/autos/Autos.java +++ b/src/main/java/frc/robot/autos/Autos.java @@ -93,12 +93,20 @@ public Autos( Map.entry( "scoreMid", wrist - .goToAngle(Positions.OUTTAKING_MID) - .andThen( - intake - .setStateCommand(IntakeState.OUTTAKING) - .withTimeout(3) - .andThen(Commands.runOnce(() -> intake.setHasCube(false))))), + .goToAngle(Positions.SHOOT_ON_MID_FOLLOW_THRU) + .alongWith( + Commands.waitUntil(() -> wrist.pastAngle(Positions.SHOOT_ON_MID)) + .andThen(intake.setStateCommand(IntakeState.SHOOTING))) + .andThen(Commands.runOnce(() -> intake.setHasCube(false)))), + Map.entry( + "scoreHigh", + wrist + .goToAngle(Positions.SHOOT_ON_HIGH_FOLLOW_THRU) + .alongWith( + Commands.waitUntil(() -> wrist.pastAngle(Positions.SHOOT_ON_HIGH)) + .andThen(intake.setStateCommand(IntakeState.SHOOTING))) + .andThen(Commands.runOnce(() -> intake.setHasCube(false)))), + Map.entry("home", wrist.getHomeCommand().withTimeout(3)), Map.entry( "intakeCube",