diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..3b8574c --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,7 @@ +{ + "robotWidth": 0.44, + "robotLength": 0.47, + "holonomicMode": true, + "generateJSON": false, + "generateCSV": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CableProtector.path b/src/main/deploy/pathplanner/CableProtector.path new file mode 100644 index 0000000..09a1514 --- /dev/null +++ b/src/main/deploy/pathplanner/CableProtector.path @@ -0,0 +1,227 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6585581889112497, + "y": 0.6965692563863765 + }, + "prevControl": null, + "nextControl": { + "x": 2.6585581889112495, + "y": 0.6965692563863765 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 0.7 + }, + "prevControl": { + "x": 2.2457598839377724, + "y": 0.7 + }, + "nextControl": { + "x": 4.505066359960422, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.524769391419125, + "y": 0.7 + }, + "prevControl": { + "x": 3.9750315240726595, + "y": 0.7 + }, + "nextControl": { + "x": 5.074507258765594, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.5231238154784, + "y": 1.073401804923269 + }, + "prevControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "nextControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.49051188700668, + "y": 0.9249526191360088 + }, + "prevControl": { + "x": 4.684637745343868, + "y": 0.8650019864142308 + }, + "nextControl": { + "x": 4.296386028669493, + "y": 0.9849032518577867 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 0.5 + } + }, + { + "anchorPoint": { + "x": 6.8200221870529205, + "y": 1.8841627426844594 + }, + "prevControl": { + "x": 6.842860523327884, + "y": 1.5986835392474208 + }, + "nextControl": { + "x": 6.842860523327884, + "y": 1.5986835392474208 + }, + "holonomicAngle": 53.9726266148967, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm " + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.444835214456754, + "y": 1.1190784774731937 + }, + "prevControl": { + "x": 6.123452930666544, + "y": 0.0685150088248904 + }, + "nextControl": null, + "holonomicAngle": -16.927513064146837, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "conepickup" + ] + }, + { + "position": 4.01, + "names": [ + "holdstow" + ] + }, + { + "position": 4.101818181818184, + "names": [ + "conepickup" + ] + }, + { + "position": 3.02, + "names": [ + "shootcone" + ] + }, + { + "position": 5.01, + "names": [ + "holdstow" + ] + }, + { + "position": 5.410909090909058, + "names": [ + "shootcone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CableProtectorCharge.path b/src/main/deploy/pathplanner/CableProtectorCharge.path new file mode 100644 index 0000000..cb72e4c --- /dev/null +++ b/src/main/deploy/pathplanner/CableProtectorCharge.path @@ -0,0 +1,221 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6585581889112497, + "y": 0.6965692563863765 + }, + "prevControl": null, + "nextControl": { + "x": 2.6585581889112495, + "y": 0.6965692563863765 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 0.7 + }, + "prevControl": { + "x": 2.2457598839377724, + "y": 0.7 + }, + "nextControl": { + "x": 4.505066359960422, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.524769391419125, + "y": 0.7 + }, + "prevControl": { + "x": 3.9750315240726595, + "y": 0.7 + }, + "nextControl": { + "x": 5.074507258765594, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.5231238154784, + "y": 1.073401804923269 + }, + "prevControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "nextControl": { + "x": 6.180548771353952, + "y": 1.15904556595438 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.49051188700668, + "y": 0.9249526191360088 + }, + "prevControl": { + "x": 4.684637745343868, + "y": 0.8650019864142308 + }, + "nextControl": { + "x": 4.296386028669493, + "y": 0.9849032518577867 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 0.5 + } + }, + { + "anchorPoint": { + "x": 6.911375532152774, + "y": 2.021192760334239 + }, + "prevControl": { + "x": 6.922794700290257, + "y": 1.3817193446352711 + }, + "nextControl": { + "x": 6.899956364015291, + "y": 2.660666176033206 + }, + "holonomicAngle": 53.9726266148967, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm " + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.405690913945931, + "y": 2.8433728662329125 + }, + "prevControl": { + "x": 5.575332860067431, + "y": 3.1973670784948407 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "conepickup" + ] + }, + { + "position": 4.01, + "names": [ + "conepickup" + ] + }, + { + "position": 3.02, + "names": [ + "shootcone" + ] + }, + { + "position": 4.01, + "names": [ + "conepickup" + ] + }, + { + "position": 5.01, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CableProtectorDoubleHigh.path b/src/main/deploy/pathplanner/CableProtectorDoubleHigh.path new file mode 100644 index 0000000..295617c --- /dev/null +++ b/src/main/deploy/pathplanner/CableProtectorDoubleHigh.path @@ -0,0 +1,214 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6928156933236946, + "y": 1.0391443005108238 + }, + "prevControl": null, + "nextControl": { + "x": 2.692815693323695, + "y": 1.0391443005108238 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placehigh", + "stowarm", + "switchcone" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 0.7 + }, + "prevControl": { + "x": 2.2457598839377724, + "y": 0.7 + }, + "nextControl": { + "x": 4.505066359960422, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "switchcone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.524769391419125, + "y": 0.7 + }, + "prevControl": { + "x": 3.9750315240726595, + "y": 0.7 + }, + "nextControl": { + "x": 5.074507258765594, + "y": 0.7 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.534542983615882, + "y": 0.8678567784486001 + }, + "prevControl": { + "x": 6.191967939491434, + "y": 0.9535005394797109 + }, + "nextControl": { + "x": 6.191967939491434, + "y": 0.9535005394797109 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.5361885595566065, + "y": 0.8107609377611926 + }, + "prevControl": { + "x": 4.730314417893793, + "y": 0.7508103050394148 + }, + "nextControl": { + "x": 4.34206270121942, + "y": 0.8707115704829705 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 0.1 + } + }, + { + "anchorPoint": { + "x": 3.3828525776709673, + "y": 0.7993417696237097 + }, + "prevControl": { + "x": 3.2857896485023743, + "y": 0.8293170859845989 + }, + "nextControl": { + "x": 3.4799155068395606, + "y": 0.7693664532628208 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.85, + "y": 0.55 + }, + "prevControl": { + "x": 2.6950184421736356, + "y": 0.515742495587556 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow", + "switchcone" + ] + }, + { + "position": 3.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "conepickup" + ] + }, + { + "position": 5.95, + "names": [ + "goarm" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FullAuto.path b/src/main/deploy/pathplanner/FullAuto.path new file mode 100644 index 0000000..6eacc24 --- /dev/null +++ b/src/main/deploy/pathplanner/FullAuto.path @@ -0,0 +1,182 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.7384923658736207, + "y": 5.070110653041819 + }, + "prevControl": null, + "nextControl": { + "x": 3.165888383058816, + "y": 5.241398175104043 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.510571414305222 + }, + "prevControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "nextControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.7499115340111024, + "y": 4.396379732930406 + }, + "prevControl": { + "x": 3.554140099733191, + "y": 5.344170688341377 + }, + "nextControl": { + "x": 3.554140099733191, + "y": 5.344170688341377 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.0 + } + }, + { + "anchorPoint": { + "x": 6.637315496853216, + "y": 3.3686546005570643 + }, + "prevControl": { + "x": 6.561774940844737, + "y": 4.426222384675781 + }, + "nextControl": { + "x": 6.705830505678105, + "y": 2.409444477008612 + }, + "holonomicAngle": -38.290163192243014, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.497044259045783, + "y": 3.11743290153247 + }, + "prevControl": { + "x": 3.679750949245486, + "y": 3.083175397120026 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.1, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "isReversed": null, + "markers": [ + { + "position": 0.4, + "names": [ + "cubepickup" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + }, + { + "position": 0.0, + "names": [ + "switchcube", + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.3563636363636347, + "names": [ + "conepickup" + ] + }, + { + "position": 3.025454545454368, + "names": [ + "holdstow" + ] + }, + { + "position": 1.8909090909090749, + "names": [ + "goarm" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MiddleOnePiece.path b/src/main/deploy/pathplanner/MiddleOnePiece.path new file mode 100644 index 0000000..c374bd4 --- /dev/null +++ b/src/main/deploy/pathplanner/MiddleOnePiece.path @@ -0,0 +1,85 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 2.729181184858097 + }, + "prevControl": null, + "nextControl": { + "x": 2.6699773570487313, + "y": 2.729181184858097 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placehigh", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.963584576741804, + "y": 2.6606661760332067 + }, + "prevControl": { + "x": 5.038631957605795, + "y": 2.7177620167206156 + }, + "nextControl": { + "x": 5.038631957605795, + "y": 2.7177620167206156 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8167809668952675, + "y": 2.740600352995578 + }, + "prevControl": { + "x": 4.650380240931421, + "y": 2.740600352995578 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.01, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MiddleTwoPiece.path b/src/main/deploy/pathplanner/MiddleTwoPiece.path new file mode 100644 index 0000000..eb360d3 --- /dev/null +++ b/src/main/deploy/pathplanner/MiddleTwoPiece.path @@ -0,0 +1,94 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 2.729181184858097 + }, + "prevControl": null, + "nextControl": { + "x": 2.6699773570487313, + "y": 2.729181184858097 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placehigh", + "stowarm", + "switchcone" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.128339726764924, + "y": 2.1696419461214993 + }, + "prevControl": { + "x": 8.277735991679151, + "y": 2.545187260400405 + }, + "nextControl": { + "x": 5.975003744879285, + "y": 1.7928093975846067 + }, + "holonomicAngle": -29.7448812969423, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9195534801326013, + "y": 2.649247007895725 + }, + "prevControl": { + "x": 2.9195534801326013, + "y": 2.649247007895725 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.5527272727272412, + "names": [ + "conepickup" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..558f413 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.046809905585623, + "y": 3.836840494193809 + }, + "prevControl": null, + "nextControl": { + "x": 3.9272190852074016, + "y": 4.376782616813096 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Patha.path b/src/main/deploy/pathplanner/Patha.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/Patha.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ThreePiece.path b/src/main/deploy/pathplanner/ThreePiece.path new file mode 100644 index 0000000..03a2d8c --- /dev/null +++ b/src/main/deploy/pathplanner/ThreePiece.path @@ -0,0 +1,189 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 4.921661467254559 + }, + "prevControl": null, + "nextControl": { + "x": 3.0973733742339267, + "y": 5.092948989316782 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.510571414305222 + }, + "prevControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "nextControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8983607197983627, + "y": 4.4077989010678875 + }, + "prevControl": { + "x": 3.531301763458228, + "y": 5.013014812354411 + }, + "nextControl": { + "x": 3.531301763458228, + "y": 5.013014812354411 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.0 + } + }, + { + "anchorPoint": { + "x": 6.5231238154784, + "y": 3.5627804588942515 + }, + "prevControl": { + "x": 5.792297054679579, + "y": 4.396379732930407 + }, + "nextControl": { + "x": 5.792297054679579, + "y": 4.396379732930407 + }, + "holonomicAngle": -24.443954780416572, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.594929976184739, + "y": 4.4077989010678875 + }, + "prevControl": { + "x": 4.022325993369936, + "y": 4.430637237342851 + }, + "nextControl": null, + "holonomicAngle": -167.73522627210778, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcube", + "placelow", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "switchcube", + "holdstow" + ] + }, + { + "position": 0.4, + "names": [ + "cubepickup" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.36, + "names": [ + "cubepickup" + ] + }, + { + "position": 3.03, + "names": [ + "holdstow" + ] + }, + { + "position": 3.8399999999998844, + "names": [ + "placelow" + ] + }, + { + "position": 1.890909090909075, + "names": [ + "goarm" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ThreePieceCone.path b/src/main/deploy/pathplanner/ThreePieceCone.path new file mode 100644 index 0000000..e41d259 --- /dev/null +++ b/src/main/deploy/pathplanner/ThreePieceCone.path @@ -0,0 +1,185 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 5.0815298211793 + }, + "prevControl": null, + "nextControl": { + "x": 3.0973733742339267, + "y": 5.2528173432415235 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placemid", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.5 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.510571414305222 + }, + "prevControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "nextControl": { + "x": 4.855925267406089, + "y": 4.556248086855148 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.7270731977361389, + "y": 4.259349715280627 + }, + "prevControl": { + "x": 4.159356011019714, + "y": 4.784631449604782 + }, + "nextControl": { + "x": 4.159356011019714, + "y": 4.784631449604782 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "placemid", + "stowarm", + "switchcone" + ], + "executionBehavior": "sequential", + "waitBehavior": "deadline", + "waitTime": 1.0 + } + }, + { + "anchorPoint": { + "x": 6.580219656165809, + "y": 3.4714271137943995 + }, + "prevControl": { + "x": 5.849392895366988, + "y": 4.305026387830555 + }, + "nextControl": { + "x": 5.849392895366988, + "y": 4.305026387830555 + }, + "holonomicAngle": -36.86989764584406, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "stowarm", + "switchcone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.480738294809923, + "y": 4.4648947417552955 + }, + "prevControl": { + "x": 3.90813431199512, + "y": 4.487733078030259 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "switchcone", + "placelow", + "stowarm" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.4, + "names": [ + "cubepickup" + ] + }, + { + "position": 0.0, + "names": [ + "switchcube", + "holdstow" + ] + }, + { + "position": 1.01, + "names": [ + "holdstow" + ] + }, + { + "position": 2.15, + "names": [ + "holdstow" + ] + }, + { + "position": 2.36, + "names": [ + "conepickup" + ] + }, + { + "position": 3.03, + "names": [ + "holdstow" + ] + }, + { + "position": 3.6072727272727443, + "names": [ + "placelow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ad3b4db..1c3d79c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,19 +39,138 @@ public final class Constants { public static final double coneUprightPosValue = 150; + public static final class ArmRotationValues { + + public static final double armRotStow = 5; + + public static final double armRotForHighCone = 70; + public static final double armRotForHighCube = 50; + + public static final double armRotForMidCone = 65; + public static final double armRotForMidCube = 40; + + public static final double armRotForLowCone = 5; + public static final double armRotForLowCube = 15; + + public static final double armRotForConePickup = 3; + public static final double armRotForCubePickup = 3; + + public static final double armRotForShelfCone = 105;//120; + public static final double armRotForShelfCube = 110; + + public static final double armRotRevHighCone = 141; + public static final double armRotRevHighCube = 145; + + public static final double armRotRevMidCone = 135; + public static final double armRotRevMidCube = 150; + + public static final double armRotRevLowCone = 30; + public static final double armRotRevLowCube = 90; + + public static final double armRotRevConePickup = 180; + public static final double armRotRevCubePickup = 170; + + public static final double armRotRevShelfCone = 95;//81; + public static final double armRotRevShelfCube = 120; + + public static final double framePerimeter = 85; + } + + public static final class ArmExtendValues { + public static final double armExtendStow = 500; + + public static final double armExtendConePickup = 7000; + public static final double armExtendCubePickup = 7000; + + + public static final double armExtendForHighCone = 48000; + public static final double armExtendForHighCube = 48000; + + public static final double armExtendForMidCone = 44000; + public static final double armExtendForMidCube = 20000; + + public static final double armExtendForLowCone = 0; + public static final double armExtendForLowCube = 0; + + public static final double armExtendForConePickup = 7000; + public static final double armExtendForCubePickup = 7000; + + public static final double armExtendForShelfCone = 0;//41000; + public static final double armExtendForShelfCube = 41000; + + + public static final double armExtendRevHighCone = 52000; + public static final double armExtendRevHighCube = 39000; + + public static final double armExtendRevMidCone = 22000; + public static final double armExtendRevMidCube = 0; + + public static final double armExtendRevLowCone = 0; + public static final double armExtendRevLowCube = 0; + + public static final double armExtendRevShelfCone = 0;//43000; + public static final double armExtendRevShelfCube = 43000; + + public static final double framePerimeter = 0; + + } + + public static final class JointRotationValues { + public static final double JointRotStowCone = 500; + public static final double JointRotStowCube = 1000; + + public static final double JointRotConePickup = 11000; + public static final double JointRotCubePickup = 10000; + + + public static final double JointRotForHighCone = 16000; + public static final double JointRotForHighCube = 17000; + + public static final double JointRotForMidCone = 43000; + public static final double JointRotForMidCube = 17000 + ; + + public static final double JointRotForLowCone = 20000; + public static final double JointRotForLowCube = 12000; + + public static final double JointRotForConePickup = 11000; + public static final double JointRotForCubePickup = 8000; + + public static final double JointRotForShelfCone = 8500;//1500; + public static final double JointRotForShelfCube = 9000; + + + public static final double JointRotRevHighCone = 12000; + public static final double JointRotRevHighCube = 2000; + + public static final double JointRotRevMidCone = 7500; + public static final double JointRotRevMidCube = 4000; + + public static final double JointRotRevLowCone = 1000; + public static final double JointRotRevLowCube = 0; + + public static final double JointRotRevShelfCone = 43200;//46000; + public static final double JointRotRevShelfCube = 0; + + public static final double framePerimeter = 0; + + } + + + public static final class Swerve { public static final int pigeonID = 1; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(24) /* 0.521 */; - public static final double wheelBase = Units.inchesToMeters(25) /* 0.622 */; + public static final double trackWidth = Units.inchesToMeters(17.5) /* 0.521 */; + public static final double wheelBase = Units.inchesToMeters(18.5) /* 0.622 */; public static final double wheelDiameter = Units.inchesToMeters(3.94); public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final double openLoopRamp = 0.2; - public static final double closedLoopRamp = 0.5; + public static final double openLoopRamp = 0.05; + public static final double closedLoopRamp = 0.61 ; public static final double driveGearRatio = (6.12 / 1.0); //8:14:1 public static final double angleGearRatio = (12.8 / 1.0); //12.8:1 @@ -91,8 +210,8 @@ public static final class Swerve { public static final double driveKA = (0.27 / 12); /* Swerve Profiling Values */ - public static final double maxSpeed = 4.5; //meters per second - public static final double maxAngularVelocity = 11.5; + public static final double maxSpeed = 10; //meters per second + public static final double maxAngularVelocity = 16; /* Neutral Modes */ public static final NeutralMode angleNeutralMode = NeutralMode.Coast; @@ -111,7 +230,7 @@ public static final class Mod0 { public static final int driveMotorID = /* 2 */5; public static final int angleMotorID = /* 1 */4; public static final int canCoderID = /* 3 */6; - public static final double angleOffset = /* 7.9 */ 304; + public static final double angleOffset = /* 274; */ 15.22; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -121,7 +240,7 @@ public static final class Mod1 { public static final int driveMotorID = /* 11 */11; public static final int angleMotorID = /* 10 */10; public static final int canCoderID = /* 12 */12; - public static final double angleOffset = /* 74 */218 ; + public static final double angleOffset = /* 240 ; */ 219.02; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -131,7 +250,7 @@ public static final class Mod2 { public static final int driveMotorID = /* 5 */2; public static final int angleMotorID = /* 4 */1; public static final int canCoderID = /* 6 */3; - public static final double angleOffset = /* 307 */312; + public static final double angleOffset = /* 319; */ 311.66; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -141,7 +260,7 @@ public static final class Mod3 { public static final int driveMotorID = /* 8 */8; public static final int angleMotorID = /* 7 */7; public static final int canCoderID = /* 9 */9; - public static final double angleOffset = /* 146 */ 289 ; + public static final double angleOffset = /* 305 ; */ 285.53; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -231,8 +350,8 @@ static class VisionConstants { new Rotation3d( 0, 0, Math.toRadians(45))); - static final String camera1Name = "OV9281-01"; - static final String camera2Name = "Cam_2"; + static final String camera1Name = "RearCam"; + static final String camera2Name = "FrontCam"; static final String frontCamName = "HD_USB_Camera"; } diff --git a/src/main/java/frc/robot/GlobalVariables.java b/src/main/java/frc/robot/GlobalVariables.java index e57ff04..ecd191c 100644 --- a/src/main/java/frc/robot/GlobalVariables.java +++ b/src/main/java/frc/robot/GlobalVariables.java @@ -11,7 +11,10 @@ public class GlobalVariables { public static int tag; public static int rowSelect; public static int height; - + public static boolean robotDirection; + public static double gripperConePlacement; + public static double armRotGoal = 100; + public static double armExtendGoal; public static enum StationSelect{ CLOSE_RAMP, LEFT_SHELF, LEFT_RAMP, RIGHT_SHELF, RIGHT_RAMP } @@ -20,17 +23,18 @@ public static enum StationSelect{ public static enum ArmPositions{ - STOWED, - GROUND_PICKUP, - GROUND_SCORE, + STOWED_ADAPTIVE, + GROUND_PICKUP_ADAPTIVE, + GROUND_SCORE_ADAPTIVE, MID_SCORE_CUBE, MID_SCORE_CONE, HIGH_SCORE_CUBE, HIGH_SCORE_CONE, - SHELF_PICKUP, + SHELF_PICKUP_ADAPTIVE, MID_SCORE_ADAPTIVE, HIGH_SCORE_ADAPTIVE, - CONE_UPRIGHT + CONE_UPRIGHT, + FRAME_PERIMETER } public static ArmPositions armPosition; diff --git a/src/main/java/frc/robot/PhotonCameraWrapper.java b/src/main/java/frc/robot/PhotonCameraWrapper.java index 3d64c41..4fe8956 100644 --- a/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -17,9 +17,8 @@ /** Add your docs here. */ public class PhotonCameraWrapper { - public PhotonCamera camera1; - public PhotonCamera camera2; public PhotonCamera frontCam; + public PhotonCamera rearCam; public PhotonPoseEstimator photonPoseEstimator; public PhotonPoseEstimator photonPoseEstimator2; @@ -28,52 +27,109 @@ public PhotonCameraWrapper() { AprilTagFieldLayout aprilTagFieldLayout; try { aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); - camera1 = new PhotonCamera(VisionConstants.camera1Name); - camera2 = new PhotonCamera(VisionConstants.camera2Name); + frontCam = new PhotonCamera(VisionConstants.camera1Name); + rearCam = new PhotonCamera(VisionConstants.camera2Name); - frontCam = new PhotonCamera(VisionConstants.frontCamName); - photonPoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camera2, VisionConstants.robotToCam1); - photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camera1, VisionConstants.robotToCam2); + // photonPoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, frontCam, VisionConstants.robotToCam1); + // photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, org.photonvision.PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, rearCam, VisionConstants.robotToCam2); } catch (Exception e) { System.out.println(e); } } - public void frontCamPipeline(int pipelineIndex){ - frontCam.setPipelineIndex(pipelineIndex);; +// public void frontCamPipeline(int pipelineIndex){ +// frontCam.setPipelineIndex(pipelineIndex);; +// } + +public void cameraPipelines(int pipeline){ + frontCam.setPipelineIndex(pipeline); + rearCam.setPipelineIndex(pipeline); +} + +public double getFrontCamOffset(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 9; + + + return result.getBestTarget().getYaw(); + +} + +public double getRearCamOffset(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()){ + return 9; + } + else{ + + + return result.getBestTarget().getYaw(); } - public double getYOffset(){ - var result = frontCam.getLatestResult(); +} + +public double getFrontCamDistance(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 0; + + + return result.getBestTarget().getBestCameraToTarget().getX(); + +} - if (!result.hasTargets()) - return 0; +public double getRearCamDistance(){ + var result = frontCam.getLatestResult(); + + if (!result.hasTargets()) + return 0; + + + return result.getBestTarget().getBestCameraToTarget().getX(); + +} + + + + + + + + // public double getYOffset(){ + // var result = frontCam.getLatestResult(); + + // if (!result.hasTargets()) + // return 0; - return result.getBestTarget().getYaw(); + // return result.getBestTarget().getYaw(); - } + // } - public double AutoGetYOffset(){ - var result = frontCam.getLatestResult(); + // public double AutoGetYOffset(){ + // var result = frontCam.getLatestResult(); - if (!result.hasTargets()) - return 0.5; + // if (!result.hasTargets()) + // return 0.5; - return result.getBestTarget().getYaw(); + // return result.getBestTarget().getYaw(); - } + // } - public double AutoTurnTOCOne(){ - var result = frontCam.getLatestResult(); + // public double AutoTurnTOCOne(){ + // var result = frontCam.getLatestResult(); - if (!result.hasTargets()) - return 0.5; + // if (!result.hasTargets()) + // return 0.5; - return result.getBestTarget().getYaw(); + // return result.getBestTarget().getYaw(); - } + // } public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { if (photonPoseEstimator == null || photonPoseEstimator2 == null) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ebec9ad..064a057 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,12 +5,23 @@ package frc.robot; + +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.math.Conversions; import frc.robot.subsystems.CANdleSub; /** @@ -28,6 +39,13 @@ public class Robot extends TimedRobot { private CANdleSub candlesub; + private PowerDistribution pdh; + DigitalInput input = new DigitalInput(0); + DigitalInput inputb = new DigitalInput(1); + boolean resetFlag = false; + private Joystick panel = new Joystick(2); + + // PhotonCamera camera = new PhotonCamera("photonvision"); @@ -39,6 +57,7 @@ public class Robot extends TimedRobot { public void robotInit() { ctreConfigs = new CTREConfigs(); candlesub = new CANdleSub(); + pdh = new PowerDistribution(1, ModuleType.kRev); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -53,9 +72,16 @@ public void robotInit() { */ @Override public void robotPeriodic() { - m_robotContainer.s_Swerve.updateOdometry(); - + boolean conevcube; CommandScheduler.getInstance().run(); + if(GlobalVariables.gamePiece == 0){ + conevcube = true; + } + else{ + conevcube = false; + } + SmartDashboard.putBoolean("Game Piece", conevcube); + m_robotContainer.s_Swerve.updateRobotDirection(); } /** This function is called once each time the robot enters Disabled mode. */ @@ -64,28 +90,52 @@ public void disabledInit() {} @Override public void disabledPeriodic() { + if(input.get()){ + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Coast); + } + else{ + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); + + } + + if (inputb.get() != resetFlag) { + resetFlag = inputb.get(); + m_robotContainer.armSub.homeGripperJointPos(); + m_robotContainer.armSub.homeTelescopePosition(); + } candlesub.disabledLed(); + pdh.setSwitchableChannel(false); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); + } /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + m_robotContainer.s_Swerve.updateOdometry(); + + } @Override public void teleopInit() { - m_robotContainer.s_Swerve.normalizeOdometry(); - m_robotContainer.s_Swerve.autoZeroGyro(); + m_robotContainer.armSub.ArmBrakeMode(NeutralMode.Brake); + + // m_robotContainer.s_Swerve.normalizeOdometry(); + // m_robotContainer.s_Swerve.autoZeroGyro(); + // m_robotContainer.s_Swerve.zeroGyro(); + // m_robotContainer.s_Swerve.resetOdometry(new Pose2d(Units.inchesToMeters(72), Units.inchesToMeters(176), new Rotation2d(0))); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -98,8 +148,16 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { + if(panel.getRawButton(17)){ + GlobalVariables.gamePiece = 0; + } + else{ + GlobalVariables.gamePiece = 1; + } Alliance alliance; alliance = DriverStation.getAlliance(); +m_robotContainer.s_Swerve.updateOdometry(); +pdh.setSwitchableChannel(false); // System.out.println(alliance); // candlesub.enabledLed(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21a2b6e..f81d547 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,16 @@ package frc.robot; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.auto.PIDConstants; +import com.pathplanner.lib.auto.SwerveAutoBuilder; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -18,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; @@ -27,7 +38,6 @@ import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; -import frc.robot.subsystems.Gripper.GripperState; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -47,28 +57,35 @@ public class RobotContainer { private final int rotationAxis = 2; /* Driver Buttons */ - private final Trigger LeftLittleButton = new JoystickButton(driver, 7); - private final Trigger RightLittleButton = new JoystickButton(driver, 8); - private final Trigger buttonX = new JoystickButton(driver, 3); - private final Trigger buttonY = new JoystickButton(driver, 4); - private final Trigger buttonB = new JoystickButton(driver, 2); - private final Trigger buttonA = new JoystickButton(driver, 1); - private final Trigger leftBumper = new JoystickButton(driver, 5); - private final Trigger rightBumper = new JoystickButton(driver, 6); private final Trigger leftJoy = new JoystickButton(joystickPanel, 1); + private final Trigger rightJoy = new JoystickButton(joystickPanel, 2); public final Trigger operatorDeploy = new JoystickButton(operatorPanel, 11); - private final Trigger operatorShoot = new JoystickButton(operatorPanel, 3); - private final Trigger operatorLeftEmpty = new JoystickButton(operatorPanel, 9); - private final Trigger operatorRightEmpty = new JoystickButton(operatorPanel, 12); - private final Trigger operatorLatch = new JoystickButton(operatorPanel, 7); - private final Trigger operatorHook = new JoystickButton(operatorPanel, 8); - private final Trigger operatorIntakeUp = new JoystickButton(operatorPanel, 13); - private final Trigger operatorIntakeDown = new JoystickButton(operatorPanel, 14); - private final Trigger operatorSpinup = new JoystickButton(operatorPanel, 1); - private final Trigger operatorExtraRight = new JoystickButton(operatorPanel, 15); - private final Trigger xboxRightJoyButton = new JoystickButton(driver, 10); + + + private final Trigger panelRollers = new JoystickButton(operatorPanel, 10); + private final Trigger panelShelf = new JoystickButton(operatorPanel, 9); + private final Trigger panelHigh = new JoystickButton(operatorPanel, 13); + private final Trigger panelMid = new JoystickButton(operatorPanel, 11); + private final Trigger panelLow = new JoystickButton(operatorPanel, 3); + private final Trigger panelStow = new JoystickButton(operatorPanel, 5); + private final Trigger panelGround = new JoystickButton(operatorPanel, 16); + private final Trigger panelGPSwitch = new JoystickButton(joystickPanel, 17); + private final Trigger panelRelease = new JoystickButton(operatorPanel, 15); + private final Trigger panelEmptyRight = new JoystickButton(operatorPanel, 12); + private final Trigger panelEmptyLeft = new JoystickButton(operatorPanel, 14); + private final Trigger panelLED1 = new JoystickButton(operatorPanel, 4); + private final Trigger panelLED2 = new JoystickButton(operatorPanel, 2); + private final Trigger panelLED3 = new JoystickButton(operatorPanel, 7); + private final Trigger panelLED4 = new JoystickButton(operatorPanel, 6); + private final Trigger panelLED5 = new JoystickButton(operatorPanel, 1); + private final Trigger panelLock = new JoystickButton(operatorPanel, 8); + + + + + // private final Trigger TopLeftSwitch = new JoystickButton(operatorPanel) public final Swerve s_Swerve = new Swerve(); public final ArmSub armSub = new ArmSub(); @@ -76,11 +93,10 @@ public class RobotContainer { public final CANdleSub candle = new CANdleSub(); public SendableChooser m_chooser = new SendableChooser<>(); - private final SequentialCommandGroup leftSideBlue = new LeftSideAuto(s_Swerve, armSub, gripper); - private final SequentialCommandGroup rightSideRed = new LeftSideAutoRed(s_Swerve, armSub, gripper); - private final SequentialCommandGroup balanceBlue = new BalanceAuto(s_Swerve, armSub, gripper); - private final SequentialCommandGroup balanceRed = new BalanceAutoRed(s_Swerve, armSub, gripper); - + // private final SequentialCommandGroup leftSideBlue = new LeftSideAuto(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup rightSideRed = new LeftSideAutoRed(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup balanceBlue = new BalanceAuto(s_Swerve, armSub, gripper); + // private final SequentialCommandGroup balanceRed = new BalanceAutoRed(s_Swerve, armSub, gripper); /* Subsystems */ @@ -95,20 +111,91 @@ public RobotContainer() { operatorPanel.setOutput(10, true ); operatorPanel.setOutput(13, true ); operatorPanel.setOutput(16, true ); + operatorPanel.setOutput(19, true ); + + operatorPanel.setOutput(3, true ); + + operatorPanel.setOutput(4, true ); + operatorPanel.setOutput(7, true ); + operatorPanel.setOutput(8, true ); + operatorPanel.setOutput(9, true ); + operatorPanel.setOutput(11, true ); + operatorPanel.setOutput(12, true ); + operatorPanel.setOutput(14, true ); + operatorPanel.setOutput(15, true ); + operatorPanel.setOutput(17, true ); + operatorPanel.setOutput(18, true ); + + HashMap eventMap = new HashMap<>(); + eventMap.put("marker1", new PrintCommand("Passed marker 1")); + eventMap.put("goarm", new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE)); + eventMap.put("stowarm", new AutoAdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); + eventMap.put("placehigh", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE)); + eventMap.put("switchcone", new ConeVCube(0)); + eventMap.put("switchcube", new ConeVCube(1)); + eventMap.put("cubepickup", new AutoGroundIntake(armSub, gripper, 1) ); + eventMap.put("conepickup", new AutoGroundIntake(armSub, gripper, 0) ); + eventMap.put("placemid", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE)); + eventMap.put("holdstow", new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); + eventMap.put("balance", new AutoBalanceStation(s_Swerve)); + eventMap.put("placelow", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.GROUND_SCORE_ADAPTIVE)); + eventMap.put("shootcone", new AutoShootCone(armSub, gripper)); - m_chooser.setDefaultOption("Balance Blue", balanceBlue); - m_chooser.addOption("Balance Red", balanceRed); - m_chooser.addOption("Left Blue", leftSideBlue); - m_chooser.addOption("Right Red", rightSideRed); + SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( + s_Swerve::getPose, // Pose2d supplier + s_Swerve::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto + Constants.Swerve.swerveKinematics, // SwerveDriveKinematics + new PIDConstants(5.0, 0.0, 0.0), // PID constants to correct for translation error (used to create the X and Y PID controllers) + new PIDConstants(1.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) + s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem + eventMap, + true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + s_Swerve // The drive subsystem. Used to properly set the requirements of path following commands + ); + List CableProtector = PathPlanner.loadPathGroup("CableProtector", new PathConstraints(3 , 3), new PathConstraints(0.5, 1),new PathConstraints(3 , 3)); + List ThreePiece = PathPlanner.loadPathGroup("ThreePieceCone", new PathConstraints(3, 3)); + List TwoPieceClimb = PathPlanner.loadPathGroup("FullAuto", new PathConstraints(3, 3),new PathConstraints(3, 3),new PathConstraints(3, 3),new PathConstraints(3, 3), new PathConstraints(2, 2)); + List MiddleOnePiece = PathPlanner.loadPathGroup("MiddleOnePiece", new PathConstraints(2, 2)); + List MiddleTwoPiece = PathPlanner.loadPathGroup("MiddleTwoPiece", new PathConstraints(2, 2)); + List CableProtectorClimb = PathPlanner.loadPathGroup("CableProtectorCharge", new PathConstraints(3 , 3), new PathConstraints(0.5, 1),new PathConstraints(3 , 3)); + List CableProtectorDoubleHigh = PathPlanner.loadPathGroup("CableProtectorDoubleHigh", new PathConstraints(3.5 , 3), new PathConstraints(0.5, 1),new PathConstraints(3.5 , 3),new PathConstraints(3.3 , 3.3),new PathConstraints(0.5, 1),new PathConstraints(3.0 , 3.0)); + + + Command cCableProtector = autoBuilder.fullAuto(CableProtector); + Command cThreePiece = autoBuilder.fullAuto(ThreePiece); + Command cTwoPieceClimb = autoBuilder.fullAuto(TwoPieceClimb); + Command cMiddleOnePiece = autoBuilder.fullAuto(MiddleOnePiece).andThen(new AutoBalanceStation(s_Swerve)); + Command cMiddleTwoPiece = autoBuilder.fullAuto(MiddleTwoPiece).andThen(new AutoBalanceStation(s_Swerve)); + Command cCableProtectorClimb = autoBuilder.fullAuto(CableProtectorClimb); + Command cCableProtectorDoubleHigh = autoBuilder.fullAuto(CableProtectorDoubleHigh); + + + + m_chooser.setDefaultOption("Cable Protector Shoot 3", cCableProtector); + m_chooser.addOption("Three Piece", cThreePiece); + m_chooser.addOption("Two Piece Climb", cTwoPieceClimb); + m_chooser.addOption("Middle One Piece", cMiddleOnePiece); + m_chooser.addOption("Mid!dle Two Piece", cMiddleTwoPiece); + m_chooser.addOption("Cable Protector Climb", cCableProtectorClimb); + m_chooser.addOption("Cable Protector Double High", cCableProtectorDoubleHigh); + + // m_chooser.addOption("Left Blue", leftSideBlue); + // m_chooser.addOption("Right Red", rightSideRed); SmartDashboard.putData(m_chooser); - s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, fieldRelative, openLoop). - withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, fieldRelative, openLoop).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + //s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver, 1,0,4,true,true)); + // armSub.setDefaultCommand(new HoldArmPos(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); //candle.setDefaultCommand(new InstantCommand(() -> candle.setColor(operatorExtraRight.getAsBoolean()))); + // armSub.setDefaultCommand(new MoveJointTemp(armSub, operatorPanel)); + // armSub.setDefaultCommand(new ArmExtendTest(armSub, operatorPanel)); + // armSub.setDefaultCommand(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + armSub.setDefaultCommand(new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); candle.setDefaultCommand(new SetLedCommand(candle)); + gripper.setDefaultCommand(new ConstantHold(gripper)); configureButtonBindings(); + } /** @@ -118,38 +205,94 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - operatorExtraRight.whileTrue(new ConeVCube(0)); - operatorExtraRight.whileFalse(new ConeVCube(1)); - - operatorIntakeUp.whileTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorIntakeUp.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorIntakeDown.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operatorIntakeDown.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorLatch.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.CONE)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorHook.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.OPEN)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorSpinup.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, false, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelRollers.whileTrue(new GripperCommand(gripper, 0.7)); //Push OUT + // panelShelf.whileTrue(new GripperCommand(gripper, -1)); // Take IN + // panelStow.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); + + + panelHigh.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelMid.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLow.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.GROUND_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelStow.onTrue(new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelGround.whileTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelShelf.whileTrue(new ShelfIntake(armSub).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelRelease.whileTrue(new AdaptiveOuttake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelRollers.whileTrue(new ManualRollers(gripper, candle).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelEmptyRight.whileTrue(new UprightConeIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelEmptyLeft.whileTrue(new SingleSubstationIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLED3.onTrue(new InstantCommand(() -> armSub.homeTelescopePosition())); + panelLED4.onTrue(new InstantCommand(() -> armSub.homeGripperJointPos())); + panelLock.whileTrue(new MoveJointTemp(armSub, operatorPanel).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + panelLED5.whileTrue(new AdaptiveArmMovement(armSub, ArmPositions.FRAME_PERIMETER).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelStow.onTrue(new AutoShootCone(armSub, gripper)); + + // panelStow.whileTrue(new FullArmPosition(armSub, 150, 50000, 9000, true)); + // panelStow.whileFalse(new FullArmPosition(armSub, 0, 0, 0, false)); + + // panelMid.whileTrue(new ArmExtendPositionTest(armSub, 50000)); + // panelMid.whileFalse(new ArmExtendPositionTest(armSub, 0)); + + // panelLow.whileTrue(new ArmRotationTest(armSub, 130).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelLow.whileFalse(new ArmRotationTest(armSub, 5).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // panelGround.whileTrue(new HoldJointPos(armSub, 11000).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 0)).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 2)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + + + // panelLow.onTrue(new InstantCommand(() -> armSub.homeArmRotPos())); + + // panelStow.whileTrue(new HoldJointPos(armSub, 19).alongWith(new ArmExtendPositionTest(armSub, 5000)).alongWith(new ArmRotationTest(armSub, 0))); + // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5))); + + + + + // panelStow.whileTrue(new HoldJointPos(armSub, 15).alongWith(new ArmExtendPositionTest(armSub, 7000)).alongWith(new ArmRotationTest(armSub, 0))); + // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + // panelGround.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + + // panelStow.whileTrue(new HoldJointPos(armSub, 11000)); + // panelStow.whileFalse(new HoldJointPos(armSub, 0)); + // panelStow.whileTrue(new HoldJointPos(armSub, 30).alongWith(new ArmExtendPositionTest(armSub, 5000)).alongWith(new ArmRotationTest(armSub, 37))); + // panelStow.whileFalse(new HoldJointPos(armSub, 0).alongWith(new ArmExtendPositionTest(armSub, 0)).alongWith(new ArmRotationTest(armSub, 5))); + + + // panelGPSwitch.whileTrue(new ConeVCube(0)); + // panelGPSwitch.whileFalse(new ConeVCube(1)); + + // operatorIntakeUp.whileTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorIntakeUp.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorIntakeDown.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // operatorIntakeDown.whileFalse(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorHook.toggleOnTrue(new InstantCommand(() -> gripper.setClaw(GripperState.OPEN)).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorSpinup.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, false, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // operatorLeftEmpty.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + + // operatorHook.whileTrue(new ArmExtendTest(armSub)); rightJoy.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); - leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // leftJoy.whileTrue(new LockToGamePiece(s_Swerve, joystickPanel, translationAxis, strafeAxis, true, true, GlobalVariables.gamePiece).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + leftJoy.whileTrue(new LockToGrid(s_Swerve, joystickPanel, translationAxis, strafeAxis, rotationAxis, true, true)); + // buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + // buttonY.whileTrue(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // buttonB.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // buttonA.whileTrue(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // xboxRightJoyButton.whileTrue(new HoldArmPos(armSub, ArmPositions.CONE_UPRIGHT).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - buttonX.toggleOnTrue(new HoldArmPos(armSub, ArmPositions.STOWED).withInterruptBehavior(InterruptionBehavior.kCancelSelf)); - buttonY.whileTrue(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - buttonB.whileTrue(new HoldArmPos(armSub, ArmPositions.MID_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - buttonA.whileTrue(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_ADAPTIVE).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - xboxRightJoyButton.whileTrue(new HoldArmPos(armSub, ArmPositions.CONE_UPRIGHT).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // leftBumper.toggleOnTrue(new GroundIntake(armSub, gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - leftBumper.whileTrue(new HoldArmPos(armSub, ArmPositions.SHELF_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - rightBumper.toggleOnTrue(new ShelfIntake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // leftBumper.whileTrue(new HoldArmPos(armSub, ArmPositions.SHELF_PICKUP).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + // rightBumper.toggleOnTrue(new ShelfIntake(gripper).withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - operatorDeploy.onTrue(new TurnInPlace(s_Swerve, true, false, 0)); + // operatorDeploy.onTrue(new TurnInPlace(s_Swerve, true, false, 0)); + // operatorDeploy.onTrue(new CustomSwerveTrajectory(s_Swerve, 10, 4, 0, 3, 10)); // operatorRightEmpty.onTrue(new BalanceAuto(s_Swerve)); // TODO RED SIDE // LeftLittleButton.onTrue(new AutoPlaceRed(s_Swerve, armSub, gripper)) ; // TODO BLUE SIDE - LeftLittleButton.onTrue(new AutoPlaceTest2(s_Swerve, armSub, gripper)); + // LeftLittleButton.onTrue(new AutoPlaceTest2(s_Swerve, armSub, gripper)); } /* _ @@ -178,16 +321,60 @@ private void configureButtonBindings() { 　| (￣ヽ＿_ヽ_)__) 　二つ */ /** + * * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { + /* + List pathGroup = PathPlanner.loadPathGroup("CableProtector", new PathConstraints(0.1, 1),new PathConstraints(4 , 2)); + +// This is just an example event map. It would be better to have a constant, global event map +// in your code that will be used by all path following commands. +HashMap eventMap = new HashMap<>(); +eventMap.put("marker1", new PrintCommand("Passed marker 1")); +eventMap.put("goarm", new AdaptiveArmMovement(armSub, ArmPositions.MID_SCORE_ADAPTIVE)); +eventMap.put("stowarm", new AutoAdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); +eventMap.put("placehigh", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.HIGH_SCORE_ADAPTIVE)); +eventMap.put("switchcone", new ConeVCube(0)); +eventMap.put("switchcube", new ConeVCube(1)); +eventMap.put("cubepickup", new AutoGroundIntake(armSub, gripper, 1) ); +eventMap.put("conepickup", new AutoGroundIntake(armSub, gripper, 0) ); +eventMap.put("placemid", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.MID_SCORE_ADAPTIVE)); +eventMap.put("holdstow", new AdaptiveArmMovement(armSub, ArmPositions.STOWED_ADAPTIVE)); +eventMap.put("balance", new AutoBalanceStation(s_Swerve, true, false)); +eventMap.put("placelow", new AutoPlaceGamePiece(armSub, gripper, ArmPositions.GROUND_SCORE_ADAPTIVE)); + + + + + + + +// Create the AutoBuilder. This only needs to be created once when robot code starts, not every time you want to create an auto command. A good place to put this is in RobotContainer along with your subsystems. +SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder( + s_Swerve::getPose, // Pose2d supplier + s_Swerve::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto + Constants.Swerve.swerveKinematics, // SwerveDriveKinematics + new PIDConstants(5.0, 0.0, 0.0), // PID constants to correct for translation error (used to create the X and Y PID controllers) + new PIDConstants(1.5, 0.0, 0.0), // PID constants to correct for rotation error (used to create the rotation controller) + s_Swerve::setModuleStates, // Module states consumer used to output to the drive subsystem + eventMap, + true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true + s_Swerve // The drive subsystem. Used to properly set the requirements of path following commands +); + +Command fullAuto = autoBuilder.fullAuto(pathGroup); + // An ExampleCommand will run in autonomous // return new BalanceAuto(s_Swerve, armSub, gripper); // return new BalanceAutoRed(s_Swerve, armSub, gripper); - return m_chooser.getSelected(); + // return m_chooser.getSelected(); + return fullAuto;//new ButterAutoTest(s_Swerve); // return new LeftSideAuto(s_Swerve, armSub, gripper); // return new LeftSideAutoRed(s_Swerve, armSub, gripper); + */ + return m_chooser.getSelected(); } } diff --git a/src/main/java/frc/robot/autos/AutoPlaceRed.java b/src/main/java/frc/robot/autos/AutoPlaceRed.java index 265ebbb..e81d55a 100644 --- a/src/main/java/frc/robot/autos/AutoPlaceRed.java +++ b/src/main/java/frc/robot/autos/AutoPlaceRed.java @@ -1,71 +1,71 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. -package frc.robot.autos; +// package frc.robot.autos; -import java.util.List; +// import java.util.List; -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.ArmPositionCommand; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.HoldArmPosAuto; -import frc.robot.commands.Test; -import frc.robot.commands.TestRed; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; +// 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.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.ArmPositionCommand; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.HoldArmPosAuto; +// import frc.robot.commands.Test; +// import frc.robot.commands.TestRed; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoPlaceRed extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand; +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +// public class AutoPlaceRed extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand; - /** Creates a new AutoPlaceTest. */ - public AutoPlaceRed(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; +// /** Creates a new AutoPlaceTest. */ +// public AutoPlaceRed(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; - addCommands( - // new InstantCommand(() -> swerve.normalizeOdometry()), - // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new GripperStateCommand(gripper, GripperState.CONE), - // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), - // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), - // new GripperStateCommand(gripper, GripperState.OPEN) - new InstantCommand(() -> swerve.normalizeOdometry()), - new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), - new TestRed(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), - new TestRed(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) +// addCommands( +// // new InstantCommand(() -> swerve.normalizeOdometry()), +// // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new GripperStateCommand(gripper, GripperState.CONE), +// // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), +// // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), +// // new GripperStateCommand(gripper, GripperState.OPEN) +// new InstantCommand(() -> swerve.normalizeOdometry()), +// new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), +// new TestRed(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), +// new TestRed(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) - ); +// ); -// 16.54 +// // 16.54 - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/AutoPlaceTest2.java b/src/main/java/frc/robot/autos/AutoPlaceTest2.java index 7801443..2aa7940 100644 --- a/src/main/java/frc/robot/autos/AutoPlaceTest2.java +++ b/src/main/java/frc/robot/autos/AutoPlaceTest2.java @@ -1,70 +1,70 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. -package frc.robot.autos; +// package frc.robot.autos; -import java.util.List; +// import java.util.List; -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.ArmPositionCommand; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.HoldArmPosAuto; -import frc.robot.commands.Test; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; +// 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.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.ArmPositionCommand; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.HoldArmPosAuto; +// import frc.robot.commands.Test; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoPlaceTest2 extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand; +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +// public class AutoPlaceTest2 extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand; - /** Creates a new AutoPlaceTest. */ - public AutoPlaceTest2(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; +// /** Creates a new AutoPlaceTest. */ +// public AutoPlaceTest2(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; - addCommands( - // new InstantCommand(() -> swerve.normalizeOdometry()), - // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new GripperStateCommand(gripper, GripperState.CONE), - // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), - // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), - // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), - // new GripperStateCommand(gripper, GripperState.OPEN) - new InstantCommand(() -> swerve.normalizeOdometry()), - new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), - new Test(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), - new Test(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) +// addCommands( +// // new InstantCommand(() -> swerve.normalizeOdometry()), +// // new Test(swerve, 0.7, 0).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new GripperStateCommand(gripper, GripperState.CONE), +// // new HoldArmPos(armSub, ArmPositions.MID_SCORE).raceWith (new WaitCommand(1)), +// // new Test(swerve, 0, 0.7).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE)), +// // new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(0.1)), +// // new GripperStateCommand(gripper, GripperState.OPEN) +// new InstantCommand(() -> swerve.normalizeOdometry()), +// new WaitCommand(0.5).raceWith(new HoldArmPosAuto(armSub)), +// new Test(swerve, 0.6, 0, false).raceWith(new HoldArmPosAuto(armSub)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new HoldArmPosAuto(armSub)), +// new Test(swerve, 0.2, 0.7, true).raceWith(new WaitCommand(1.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE).raceWith(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP)), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).raceWith(new WaitCommand(1)) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/BalanceAuto.java b/src/main/java/frc/robot/autos/BalanceAuto.java index a776b7d..37fff26 100644 --- a/src/main/java/frc/robot/autos/BalanceAuto.java +++ b/src/main/java/frc/robot/autos/BalanceAuto.java @@ -1,111 +1,111 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoBalanceStation; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.BalanceStation; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class BalanceAuto extends SequentialCommandGroup { - Swerve swerve; - ArmSub armsub; - Gripper gripper; - Joystick controller; - - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - - - /** Creates a new AutoPlaceTest. */ - public BalanceAuto(Swerve swerve, ArmSub armsub, Gripper gripper) { - this.swerve = swerve; - this.armsub = armsub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// 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.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoBalanceStation; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.BalanceStation; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class BalanceAuto extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armsub; +// Gripper gripper; +// Joystick controller; + +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; + + +// /** Creates a new AutoPlaceTest. */ +// public BalanceAuto(Swerve swerve, ArmSub armsub, Gripper gripper) { +// this.swerve = swerve; +// this.armsub = armsub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 0.6, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 0.8, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.6, 3.25, new Rotation2d(Math.PI)), - new Pose2d(1.82, 3.25, new Rotation2d(Math.PI))), - config); - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(1.97, 3.25, new Rotation2d(Math.PI)), - new Pose2d(3.85, 3.25, new Rotation2d(Math.PI))), - configrev); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(3.85, 3.25, new Rotation2d(Math.PI)), - new Pose2d(5.4, 3.25, new Rotation2d(Math.PI))), - configrev); - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(5.4, 3.25, new Rotation2d(Math.PI)), - new Pose2d(3.7, 3.25, new Rotation2d(Math.PI))), - config); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 0.6, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 0.8, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + + + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.6, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(1.82, 3.25, new Rotation2d(Math.PI))), +// config); +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(1.97, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(3.85, 3.25, new Rotation2d(Math.PI))), +// configrev); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(3.85, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(5.4, 3.25, new Rotation2d(Math.PI))), +// configrev); +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(5.4, 3.25, new Rotation2d(Math.PI)), +// new Pose2d(3.7, 3.25, new Rotation2d(Math.PI))), +// config); @@ -113,66 +113,66 @@ public BalanceAuto(Swerve swerve, ArmSub armsub, Gripper gripper) { - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - // swerveControllerCommand3, - swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) +// addCommands( +// new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// // swerveControllerCommand3, +// swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/BalanceAutoRed.java b/src/main/java/frc/robot/autos/BalanceAutoRed.java index b39c557..e3a3ef2 100644 --- a/src/main/java/frc/robot/autos/BalanceAutoRed.java +++ b/src/main/java/frc/robot/autos/BalanceAutoRed.java @@ -1,111 +1,111 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoBalanceStation; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.BalanceStation; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class BalanceAutoRed extends SequentialCommandGroup { - Swerve swerve; - ArmSub armsub; - Gripper gripper; - Joystick controller; - - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - - - /** Creates a new AutoPlaceTest. */ - public BalanceAutoRed(Swerve swerve, ArmSub armsub, Gripper gripper) { - this.swerve = swerve; - this.armsub = armsub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// 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.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoBalanceStation; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.BalanceStation; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class BalanceAutoRed extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armsub; +// Gripper gripper; +// Joystick controller; + +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; + + +// /** Creates a new AutoPlaceTest. */ +// public BalanceAutoRed(Swerve swerve, ArmSub armsub, Gripper gripper) { +// this.swerve = swerve; +// this.armsub = armsub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 0.6, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 0.8, - 1) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.94, 3.25, new Rotation2d(0)), - new Pose2d(14.72, 3.25, new Rotation2d(0))), - config); - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(14.57, 3.25, new Rotation2d(0)), - new Pose2d(12.69, 3.25, new Rotation2d(0))), - configrev); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(12.69, 3.25, new Rotation2d(0)), - new Pose2d(11.14, 3.25, new Rotation2d(0))), - configrev); - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(11.14, 3.25, new Rotation2d(0)), - new Pose2d(12.84, 3.25, new Rotation2d(0))), - config); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 0.6, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 0.8, +// 1) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + + + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.94, 3.25, new Rotation2d(0)), +// new Pose2d(14.72, 3.25, new Rotation2d(0))), +// config); +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(14.57, 3.25, new Rotation2d(0)), +// new Pose2d(12.69, 3.25, new Rotation2d(0))), +// configrev); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(12.69, 3.25, new Rotation2d(0)), +// new Pose2d(11.14, 3.25, new Rotation2d(0))), +// configrev); +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(11.14, 3.25, new Rotation2d(0)), +// new Pose2d(12.84, 3.25, new Rotation2d(0))), +// config); @@ -113,66 +113,66 @@ public BalanceAutoRed(Swerve swerve, ArmSub armsub, Gripper gripper) { - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(0.5, 0, 0), - new PIDController(0.5, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(0.5, 0, 0), +// new PIDController(0.5, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - // swerveControllerCommand3, - swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), - new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) +// addCommands( +// new WaitCommand(0.5).raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armsub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand3.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armsub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// // swerveControllerCommand3, +// swerveControllerCommand4.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armsub, ArmPositions.STOWED)), +// new AutoBalanceStation(swerve, false, false).deadlineWith(new WaitCommand(10)) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/ButterAutoTest.java b/src/main/java/frc/robot/autos/ButterAutoTest.java new file mode 100644 index 0000000..8f10373 --- /dev/null +++ b/src/main/java/frc/robot/autos/ButterAutoTest.java @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.autos; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.CustomSwerveTrajTrap; +import frc.robot.commands.CustomSwerveTrajectory; +import frc.robot.subsystems.Swerve; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ButterAutoTest extends SequentialCommandGroup { + Swerve swerve; + /** Creates a new ButterAutoTest. */ + public ButterAutoTest(Swerve swerve) { + this.swerve = swerve; + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new CustomSwerveTrajectory(swerve, 6.86, 4.56, 0, 0.3, 4), + new CustomSwerveTrajectory(swerve, 3 , 4.97, 180, 0.3, 4), + new CustomSwerveTrajectory(swerve, 6.86, 4.56, 0, 0.3, 4), + new CustomSwerveTrajectory(swerve, 3 , 4.97, 180, 0.3, 4) + +/* new CustomSwerveTrajTrap(swerve, 6.86, 4.56, 0, 0.1, 6), +new CustomSwerveTrajTrap(swerve, 3 , 4.97, 180, 0.1, 6), +new CustomSwerveTrajTrap(swerve, 6.86, 4.56, 0, 0.1, 6), +new CustomSwerveTrajTrap(swerve, 3 , 4.97, 180, 0.1, 6) + */ + ); + } +} diff --git a/src/main/java/frc/robot/autos/LeftSideAuto.java b/src/main/java/frc/robot/autos/LeftSideAuto.java index 4d21b65..5f8f882 100644 --- a/src/main/java/frc/robot/autos/LeftSideAuto.java +++ b/src/main/java/frc/robot/autos/LeftSideAuto.java @@ -1,234 +1,234 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.commands.TurnInPlace; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class LeftSideAuto extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - Joystick controller; - -MinMax minMax; - TrajectoryConstraint constraints; - - /** Creates a new AutoPlaceTest. */ - public LeftSideAuto(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// 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.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.commands.TurnInPlace; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class LeftSideAuto extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; +// Joystick controller; + +// MinMax minMax; +// TrajectoryConstraint constraints; + +// /** Creates a new AutoPlaceTest. */ +// public LeftSideAuto(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 1.7, - 1.7) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig placeConfig = new TrajectoryConfig( - 0.95, - 1.3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 1.7, +// 1.7) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig placeConfig = new TrajectoryConfig( +// 0.95, +// 1.3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 3, - 3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig configrevdfast = new TrajectoryConfig( - 2, - 2) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(3, 5.0, new Rotation2d(Math.PI)), - new Pose2d(2.3, 5.0, new Rotation2d(Math.PI))), - placeConfig); - - Trajectory traj2 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.2, 5.15, new Rotation2d(Math.PI)), - new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), - configrev); - - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.8, 5.15, new Rotation2d(Math.PI)), - new Pose2d(4.0, 4.75, new Rotation2d(Math.PI))), - configrevdfast); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(4.0, 5.15, new Rotation2d(Math.PI)), - new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), - config); - - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(3, 5.15, new Rotation2d(Math.PI)), - new Pose2d(1.85, 5.0, new Rotation2d(Math.PI))), - config); - Trajectory traj6 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(2.1, 5.15, new Rotation2d(Math.PI)), - new Pose2d(2.8, 4.8, new Rotation2d(Math.PI))), - configrev); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 3, +// 3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig configrevdfast = new TrajectoryConfig( +// 2, +// 2) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(3, 5.0, new Rotation2d(Math.PI)), +// new Pose2d(2.3, 5.0, new Rotation2d(Math.PI))), +// placeConfig); + +// Trajectory traj2 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.2, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), +// configrev); + +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.8, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(4.0, 4.75, new Rotation2d(Math.PI))), +// configrevdfast); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(4.0, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(2.8, 5.15, new Rotation2d(Math.PI))), +// config); + +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(3, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(1.85, 5.0, new Rotation2d(Math.PI))), +// config); +// Trajectory traj6 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(2.1, 5.15, new Rotation2d(Math.PI)), +// new Pose2d(2.8, 4.8, new Rotation2d(Math.PI))), +// configrev); - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand2 = new SwerveControllerCommand( - traj2, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1.7, 0, 0), - new PIDController(1.7, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getPose, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); - - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand6 = new SwerveControllerCommand( - traj6, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); + +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand2 = new SwerveControllerCommand( +// traj2, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1.7, 0, 0), +// new PIDController(1.7, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getPose, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); + +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand6 = new SwerveControllerCommand( +// traj6, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( +// addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - - // swerveControllerCommand2.raceWith(new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - // new GripperStateCommand(gripper, GripperState.CONE).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new InstantCommand(() -> swerve.normalizeOdometry()), - swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - new TurnInPlace(swerve, true , false, 0), - new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), - new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new TurnInPlace(swerve, true , false, 178), - swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand6.raceWith(new WaitCommand(01).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), + +// // swerveControllerCommand2.raceWith(new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// // new GripperStateCommand(gripper, GripperState.CONE).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new InstantCommand(() -> swerve.normalizeOdometry()), +// swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// new TurnInPlace(swerve, true , false, 0), +// new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), +// new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new TurnInPlace(swerve, true , false, 178), +// swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand6.raceWith(new WaitCommand(01).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/autos/LeftSideAutoRed.java b/src/main/java/frc/robot/autos/LeftSideAutoRed.java index 68aed86..d0f1659 100644 --- a/src/main/java/frc/robot/autos/LeftSideAutoRed.java +++ b/src/main/java/frc/robot/autos/LeftSideAutoRed.java @@ -1,230 +1,230 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.autos; - - -import java.util.List; - -// package edu.wpi.first.math.trajectory.constraint; - -import edu.wpi.first.math.geometry.Pose2d; - -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; -import frc.robot.GlobalVariables.ArmPositions; -import frc.robot.commands.AutoLockToGamePiece; -import frc.robot.commands.AutoSwervePositions; -import frc.robot.commands.GripperStateCommand; -import frc.robot.commands.GroundIntake; -import frc.robot.commands.HoldArmPos; -import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.Test; -import frc.robot.commands.TurnInPlace; -import frc.robot.subsystems.ArmSub; -import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html - -public class LeftSideAutoRed extends SequentialCommandGroup { - Swerve swerve; - ArmSub armSub; - Gripper gripper; - SwerveControllerCommand swerveControllerCommand1; - SwerveControllerCommand swerveControllerCommand2; - SwerveControllerCommand swerveControllerCommand3; - SwerveControllerCommand swerveControllerCommand4; - SwerveControllerCommand swerveControllerCommand5; - SwerveControllerCommand swerveControllerCommand6; - Joystick controller; - -MinMax minMax; - TrajectoryConstraint constraints; - - /** Creates a new AutoPlaceTest. */ - public LeftSideAutoRed(Swerve swerve, ArmSub armSub, Gripper gripper) { - this.swerve = swerve; - this.armSub = armSub; - this.gripper = gripper; - controller = new Joystick(1); +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. + +// package frc.robot.autos; + + +// import java.util.List; + +// // package edu.wpi.first.math.trajectory.constraint; + +// import edu.wpi.first.math.geometry.Pose2d; + +// 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.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +// import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +// import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint.MinMax; +// import edu.wpi.first.wpilibj.Joystick; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants; +// import frc.robot.GlobalVariables.ArmPositions; +// import frc.robot.commands.AutoLockToGamePiece; +// import frc.robot.commands.AutoSwervePositions; +// import frc.robot.commands.GripperStateCommand; +// import frc.robot.commands.GroundIntake; +// import frc.robot.commands.HoldArmPos; +// import frc.robot.commands.TeleopSwerve; +// import frc.robot.commands.Test; +// import frc.robot.commands.TurnInPlace; +// import frc.robot.subsystems.ArmSub; +// import frc.robot.subsystems.Gripper; +// import frc.robot.subsystems.Swerve; +// import frc.robot.subsystems.Gripper.GripperState; + +// // NOTE: Consider using this command inline, rather than writing a subclass. For more +// // information, see: +// // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +// public class LeftSideAutoRed extends SequentialCommandGroup { +// Swerve swerve; +// ArmSub armSub; +// Gripper gripper; +// SwerveControllerCommand swerveControllerCommand1; +// SwerveControllerCommand swerveControllerCommand2; +// SwerveControllerCommand swerveControllerCommand3; +// SwerveControllerCommand swerveControllerCommand4; +// SwerveControllerCommand swerveControllerCommand5; +// SwerveControllerCommand swerveControllerCommand6; +// Joystick controller; + +// MinMax minMax; +// TrajectoryConstraint constraints; + +// /** Creates a new AutoPlaceTest. */ +// public LeftSideAutoRed(Swerve swerve, ArmSub armSub, Gripper gripper) { +// this.swerve = swerve; +// this.armSub = armSub; +// this.gripper = gripper; +// controller = new Joystick(1); - // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) - TrajectoryConfig config = new TrajectoryConfig( - 1.7, - 1.7) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig placeConfig = new TrajectoryConfig( - 0.95, - 1.3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); - config.addConstraint(new CentripetalAccelerationConstraint(0.3)); +// // constraints.getMinMaxAccelerationMetersPerSecondSq(null, 0, 0) +// TrajectoryConfig config = new TrajectoryConfig( +// 1.7, +// 1.7) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig placeConfig = new TrajectoryConfig( +// 0.95, +// 1.3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(false); +// config.addConstraint(new CentripetalAccelerationConstraint(0.3)); - TrajectoryConfig configrev = new TrajectoryConfig( - 3, - 3) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - TrajectoryConfig configrevdfast = new TrajectoryConfig( - 2, - 2) - .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); - configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); - - Trajectory traj1 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.54, 5.0, new Rotation2d(0)), - new Pose2d(14.24, 5.0, new Rotation2d(0))), - placeConfig); - - Trajectory traj2 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(14.34, 5.15, new Rotation2d(0)), - new Pose2d(13.74, 5.15, new Rotation2d(0))), - configrev); - - Trajectory traj3 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.74, 5.15, new Rotation2d(0)), - new Pose2d(12.54, 4.75, new Rotation2d(0))), - configrevdfast); - - Trajectory traj4 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(12.54, 5.15, new Rotation2d(0)), - new Pose2d(13.74, 5.15, new Rotation2d(0))), - config); - - Trajectory traj5 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(13.54, 5.15, new Rotation2d(0)), - new Pose2d(14.69, 5.0, new Rotation2d(0))), - config); - Trajectory traj6 = - TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ - new Pose2d(14.44, 5.15, new Rotation2d(0)), - new Pose2d(13.74, 4.8, new Rotation2d(0))), - configrev); +// TrajectoryConfig configrev = new TrajectoryConfig( +// 3, +// 3) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// TrajectoryConfig configrevdfast = new TrajectoryConfig( +// 2, +// 2) +// .setKinematics(Constants.Swerve.swerveKinematics).setReversed(true); +// configrev.addConstraint(new CentripetalAccelerationConstraint(0.3)); + +// Trajectory traj1 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.54, 5.0, new Rotation2d(0)), +// new Pose2d(14.24, 5.0, new Rotation2d(0))), +// placeConfig); + +// Trajectory traj2 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(14.34, 5.15, new Rotation2d(0)), +// new Pose2d(13.74, 5.15, new Rotation2d(0))), +// configrev); + +// Trajectory traj3 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.74, 5.15, new Rotation2d(0)), +// new Pose2d(12.54, 4.75, new Rotation2d(0))), +// configrevdfast); + +// Trajectory traj4 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(12.54, 5.15, new Rotation2d(0)), +// new Pose2d(13.74, 5.15, new Rotation2d(0))), +// config); + +// Trajectory traj5 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(13.54, 5.15, new Rotation2d(0)), +// new Pose2d(14.69, 5.0, new Rotation2d(0))), +// config); +// Trajectory traj6 = +// TrajectoryGenerator.generateTrajectory(List.of(/* swerve.getAprilTagEstPosition(), */ +// new Pose2d(14.44, 5.15, new Rotation2d(0)), +// new Pose2d(13.74, 4.8, new Rotation2d(0))), +// configrev); - var thetaController = new ProfiledPIDController( - 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - swerveControllerCommand1 = new SwerveControllerCommand( - traj1, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand2 = new SwerveControllerCommand( - traj2, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(2, 0, 0), - new PIDController(2, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand3 = new SwerveControllerCommand( - traj3, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1.7, 0, 0), - new PIDController(1.7, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerveControllerCommand4 = new SwerveControllerCommand( - traj4, - swerve::getPose, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); - swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); - - swerveControllerCommand5 = new SwerveControllerCommand( - traj5, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); - - swerveControllerCommand6 = new SwerveControllerCommand( - traj6, - swerve::getAprilTagEstPosition, - Constants.Swerve.swerveKinematics, - new PIDController(1, 0, 0), - new PIDController(1, 0, 0), - thetaController, - swerve::setModuleStates, - swerve); +// var thetaController = new ProfiledPIDController( +// 1, 0, 0, Constants.AutoConstants.kThetaControllerConstraints); + +// thetaController.enableContinuousInput(-Math.PI, Math.PI); + +// swerveControllerCommand1 = new SwerveControllerCommand( +// traj1, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand2 = new SwerveControllerCommand( +// traj2, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(2, 0, 0), +// new PIDController(2, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand3 = new SwerveControllerCommand( +// traj3, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1.7, 0, 0), +// new PIDController(1.7, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerveControllerCommand4 = new SwerveControllerCommand( +// traj4, +// swerve::getPose, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj1); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj2); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj3); +// swerve.m_fieldSim.getObject("traj2").setTrajectory(traj4); + +// swerveControllerCommand5 = new SwerveControllerCommand( +// traj5, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); + +// swerveControllerCommand6 = new SwerveControllerCommand( +// traj6, +// swerve::getAprilTagEstPosition, +// Constants.Swerve.swerveKinematics, +// new PIDController(1, 0, 0), +// new PIDController(1, 0, 0), +// thetaController, +// swerve::setModuleStates, +// swerve); - addCommands( - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new InstantCommand(() -> swerve.normalizeOdometry()), - swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), - new TurnInPlace(swerve, true , false, 178), - new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), - new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - new TurnInPlace(swerve, true , false, 0), - swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), - new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), - new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), - swerveControllerCommand6.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), - new GripperStateCommand(gripper, GripperState.CONE), - new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) +// addCommands( +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// swerveControllerCommand1.raceWith(new HoldArmPos(armSub, ArmPositions.MID_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new InstantCommand(() -> swerve.normalizeOdometry()), +// swerveControllerCommand3.raceWith(/* new HoldArmPos(armSub, ArmPositions.STOWED) */new WaitCommand(0.5).andThen(new HoldArmPos(armSub, ArmPositions.STOWED).alongWith(new GripperStateCommand(gripper, GripperState.CONE)))), +// new TurnInPlace(swerve, true , false, 178), +// new AutoLockToGamePiece(swerve, 0, 0, false, false , 0).raceWith(new GroundIntake(armSub, gripper)), +// new WaitCommand(0.4 ).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// new TurnInPlace(swerve, true , false, 0), +// swerveControllerCommand4.raceWith(new HoldArmPos(armSub, ArmPositions.STOWED)), +// new WaitCommand(0.5).raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// swerveControllerCommand5.raceWith(new HoldArmPos(armSub, ArmPositions.HIGH_SCORE_CONE)), +// new GripperStateCommand(gripper, GripperState.OPEN).raceWith(new TeleopSwerve(swerve, controller, 0, 0, 0, false, false)), +// swerveControllerCommand6.raceWith(new WaitCommand(1).andThen(new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP))), +// new GripperStateCommand(gripper, GripperState.CONE), +// new HoldArmPos(armSub, ArmPositions.GROUND_PICKUP) - ); +// ); - } -} +// } +// } diff --git a/src/main/java/frc/robot/commands/AdaptiveArmMovement.java b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java new file mode 100644 index 0000000..0885e7a --- /dev/null +++ b/src/main/java/frc/robot/commands/AdaptiveArmMovement.java @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; + +public class AdaptiveArmMovement extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + GlobalVariables.ArmPositions armPosition; + public AdaptiveArmMovement(ArmSub armSub, GlobalVariables.ArmPositions armPosition) { + this.armSub = armSub; + this.armPosition = armPosition; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(armPosition == ArmPositions.STOWED_ADAPTIVE){ + armSub.armExtendPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getTelescopePos() <= 10000 && armSub.getGripperJointPos() <= 5000){ + armSub.armRotPresetPositions(armPosition); + + } + } + else{ + if(GlobalVariables.robotDirection == false){ + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(armPosition); + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.15){ + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getGripperJointPos()>= 10000){ + armSub.armExtendPresetPositions(armPosition); + } + } + + } + + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/GripperStateCommand.java b/src/main/java/frc/robot/commands/AdaptiveOuttake.java similarity index 64% rename from src/main/java/frc/robot/commands/GripperStateCommand.java rename to src/main/java/frc/robot/commands/AdaptiveOuttake.java index a4c1a78..ca02dac 100644 --- a/src/main/java/frc/robot/commands/GripperStateCommand.java +++ b/src/main/java/frc/robot/commands/AdaptiveOuttake.java @@ -5,45 +5,43 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Gripper.GripperState; -public class GripperStateCommand extends CommandBase { +public class AdaptiveOuttake extends CommandBase { + /** Creates a new AdaptiveOuttake. */ Gripper gripper; - GripperState state; - int timer; - /** Creates a new GripperStateCommand. */ - public GripperStateCommand(Gripper gripper, GripperState state) { + public AdaptiveOuttake(Gripper gripper) { this.gripper = gripper; - this.state = state; addRequirements(gripper); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() { - timer = 0; - gripper.setClaw(state); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - timer ++; - System.out.println("GRIPPER COMMAND EXEC"); + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(0.5); + } + else{ + gripper.moveGripper(-0.6); + } } // Called once the command ends or is interrupted. + @Override public void end(boolean interrupted) { - System.out.println("GRIPPER COMMAND END"); - + gripper.moveGripper(0); } // Returns true when the command should end. @Override public boolean isFinished() { - return timer > 7; + return false; } } diff --git a/src/main/java/frc/robot/commands/ArmExtendPositionTest.java b/src/main/java/frc/robot/commands/ArmExtendPositionTest.java new file mode 100644 index 0000000..5744fb4 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmExtendPositionTest.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class ArmExtendPositionTest extends CommandBase { + ArmSub armSub; + double position; + /** Creates a new ArmExtendPositionTest. */ + public ArmExtendPositionTest(ArmSub armSub, double position) { + this.armSub = armSub; + this.position = position; + // addRequirements(armSub); + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armSub.moveTelescopeArmPosition(position); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armSub.moveTelescopeArmPosition(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmExtendTest.java b/src/main/java/frc/robot/commands/ArmExtendTest.java new file mode 100644 index 0000000..48f6672 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmExtendTest.java @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class ArmExtendTest extends CommandBase { + ArmSub armsub; + Joystick controller; + /** Creates a new ArmExtendTest. */ + public ArmExtendTest(ArmSub armsub, Joystick controller) { + this.armsub = armsub; + this.controller = controller; + addRequirements(armsub); + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(controller.getRawButton(17)){ + armsub.moveRotArmPercentOutput(controller.getRawAxis(0)); + } + else{ + armsub.moveTelescopeArmPercentOutput(controller.getRawAxis(0)); + } + // armsub.extendArmPosition(1000); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armsub.moveRotArmPercentOutput(0); + armsub.moveTelescopeArmPercentOutput(0); + // armsub.extendArmPosition(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmPercentageCommand.java b/src/main/java/frc/robot/commands/ArmPercentageCommand.java index c5866d4..1b192fd 100644 --- a/src/main/java/frc/robot/commands/ArmPercentageCommand.java +++ b/src/main/java/frc/robot/commands/ArmPercentageCommand.java @@ -31,7 +31,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - armSub.moveArmPercentOutput(controller.getRawAxis(axis1) - controller.getRawAxis(axis2) ); + armSub.moveRotArmPercentOutput(controller.getRawAxis(axis1) - controller.getRawAxis(axis2) ); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ArmPositionCommand.java b/src/main/java/frc/robot/commands/ArmPositionCommand.java index c735966..9bfb903 100644 --- a/src/main/java/frc/robot/commands/ArmPositionCommand.java +++ b/src/main/java/frc/robot/commands/ArmPositionCommand.java @@ -32,7 +32,7 @@ public void initialize() { @Override public void execute() { timer ++; - armSub.armPresetPositions(armPos); + armSub.armRotPresetPositions(armPos); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ArmRotationTest.java b/src/main/java/frc/robot/commands/ArmRotationTest.java new file mode 100644 index 0000000..fd68f53 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmRotationTest.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class ArmRotationTest extends CommandBase { + ArmSub armSub; + double position; + /** Creates a new ArmRotationTest. */ + public ArmRotationTest(ArmSub armSub, double position) { + this.position = position; + this.armSub = armSub; + // addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armSub.moveRotArmPosition(position); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java b/src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java new file mode 100644 index 0000000..a9eb1b8 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoAdaptiveArmMovement.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; + +public class AutoAdaptiveArmMovement extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + GlobalVariables.ArmPositions armPosition; + boolean finished; + public AutoAdaptiveArmMovement(ArmSub armSub, GlobalVariables.ArmPositions armPosition) { + this.armSub = armSub; + this.armPosition = armPosition; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(armPosition == ArmPositions.STOWED_ADAPTIVE){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() <= 10000){ + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + if(armSub.getArmEncoderPosition() >= (GlobalVariables.armRotGoal - 5) && + armSub.getArmEncoderPosition() <= (GlobalVariables.armRotGoal + 5) ){ + finished = true; + } + + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() >= (GlobalVariables.armExtendGoal - 2000) && + armSub.getTelescopePos() <= (GlobalVariables.armExtendGoal + 2000) ){ + finished = true; + } + } + + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/AutoBalanceStation.java b/src/main/java/frc/robot/commands/AutoBalanceStation.java index 4e770e4..ef7ea1b 100644 --- a/src/main/java/frc/robot/commands/AutoBalanceStation.java +++ b/src/main/java/frc/robot/commands/AutoBalanceStation.java @@ -26,60 +26,33 @@ private double mapdouble(double x, double in_min, double in_max, double out_min, /** * Driver control */ - public AutoBalanceStation(Swerve s_Swerve, boolean fieldRelative, boolean openLoop) { - this.s_Swerve = s_Swerve; + public AutoBalanceStation(Swerve s_Swerve) { + this.s_Swerve = s_Swerve ; addRequirements(s_Swerve); - pidController = new PIDController(0.01 , 0, 0); - pidController.setTolerance(1); - - this.fieldRelative = fieldRelative; - this.openLoop = openLoop; + pidController = new PIDController(0.1 , 0, 0); } @Override public void execute() { - double yAxis = pidController.calculate(s_Swerve.getPitch(), 0); + double value = 0; + if (s_Swerve.getPitch() > 2.5) { + value = 1; + } + if (s_Swerve.getPitch() < -2.5) { + value = -1; + } + double yAxis = pidController.calculate(value , 0); double xAxis = 0; double rAxis = 0; - System.out.println("AUTO BALANCING "); - - /* Deadbands */ - - if (Math.abs(yAxis) > Constants.stickDeadband) { - if (yAxis > 0){ - yAxis = mapdouble(yAxis, Constants.stickDeadband, 1, 0, 1); - } else { - yAxis = mapdouble(yAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - yAxis = 0; - } - - if (Math.abs(xAxis) > Constants.stickDeadband) { - if (xAxis > 0){ - xAxis = mapdouble(xAxis, Constants.stickDeadband, 1, 0, 1); - } else { - xAxis = mapdouble(xAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - xAxis = 0; - } - - if (Math.abs(rAxis) > Constants.stickDeadband) { - if (rAxis > 0){ - rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); - } else { - rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - rAxis = 0; - } + translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); rotation = rAxis * Constants.Swerve.maxAngularVelocity; - s_Swerve.drive(translation, rotation, fieldRelative, openLoop); + s_Swerve.drive(translation, rotation, true, true); } + @Override + public boolean isFinished() { + // return pidController.atSetpoint(); + return false; + } } diff --git a/src/main/java/frc/robot/commands/AutoGroundIntake.java b/src/main/java/frc/robot/commands/AutoGroundIntake.java new file mode 100644 index 0000000..dddacfe --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoGroundIntake.java @@ -0,0 +1,85 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class AutoGroundIntake extends CommandBase { + /** Creates a new GroundIntake. */ + ArmSub armSub; + Gripper gripper; + int gamePiece; + boolean finished; + /** + * For gamepiece cone = 0, and cube = 1 + * @param armSub + * @param gripper + * @param gamePiece + */ + public AutoGroundIntake(ArmSub armSub, Gripper gripper, int gamePiece) { + this.armSub = armSub; + this.gripper = gripper; + this.gamePiece = gamePiece; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(gamePiece == 0){ + armSub.moveGripperJointPosition(31000*armSub.multiplier); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + /* if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + finished = true; + + } + else{ */ + gripper.moveGripper(-1); + + /* } */ + } + + else{ + armSub.moveGripperJointPosition(22000*armSub.multiplier); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + /* if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + finished = true; + + } + else{ */ + gripper.moveGripper(0.8); + + /* } */ + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java new file mode 100644 index 0000000..c5f4638 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoPlaceGamePiece.java @@ -0,0 +1,100 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class AutoPlaceGamePiece extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + Gripper gripper; + GlobalVariables.ArmPositions armPosition; + boolean finished; + int timer; + public AutoPlaceGamePiece(ArmSub armSub, Gripper gripper, GlobalVariables.ArmPositions armPosition) { + this.armSub = armSub; + this.armPosition = armPosition; + this.gripper = gripper; + addRequirements(armSub, gripper); + finished = false; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + timer = 0; + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(armPosition == ArmPositions.STOWED_ADAPTIVE){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos() <= 10000){ + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + } + } + else{ + + armSub.armRotPresetPositions(armPosition); + armSub.jointRotPresetPositions(armPosition); + + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(armPosition); + if(armSub.getTelescopePos()>= (GlobalVariables.armExtendGoal -500) && + armSub.getTelescopePos()<= (GlobalVariables.armExtendGoal + 500)){ + timer++; + if(/* gripper.getDistanceSensorDist() <= 12 */ timer>=5){ + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(0.7); + } + else{ + gripper.moveGripper(-0.6); + + } + if (timer >= 20){ + finished = true; + gripper.moveGripper(0); + + + } + } + else{ + gripper.moveGripper(0); + } + } + else{ + finished = false; + } + } + + + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/AutoShootCone.java b/src/main/java/frc/robot/commands/AutoShootCone.java new file mode 100644 index 0000000..b8e8707 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCone.java @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class AutoShootCone extends CommandBase { + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + Gripper gripper; + GlobalVariables.ArmPositions armPosition; + boolean finished; + int timer; + public AutoShootCone(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub, gripper); + finished = false; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + finished = false; + timer = 0; + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + armSub.moveRotArmPosition(90); + armSub.moveGripperJointPosition(11000*armSub.multiplier); + + if(armSub.getArmEncoderPosition() >= 80){ + + timer++; + if(/* gripper.getDistanceSensorDist() <= 12 */ timer>=15){ + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(1); + } + else{ + gripper.moveGripper(-1); + + } + if (timer >= 30){ + gripper.moveGripper(0); + finished = true; + + + } + } + else{ + gripper.moveGripper(0); + } + } + else{ + finished = false; + } + } + + + + + + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return finished; + } +} diff --git a/src/main/java/frc/robot/commands/AutoSwervePositions.java b/src/main/java/frc/robot/commands/AutoSwervePositions.java index c2ad2d9..6078b8a 100644 --- a/src/main/java/frc/robot/commands/AutoSwervePositions.java +++ b/src/main/java/frc/robot/commands/AutoSwervePositions.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Gripper; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; public class AutoSwervePositions extends CommandBase { Swerve swerve; diff --git a/src/main/java/frc/robot/commands/ConeVCube.java b/src/main/java/frc/robot/commands/ConeVCube.java index 6b41217..f3ccd6e 100644 --- a/src/main/java/frc/robot/commands/ConeVCube.java +++ b/src/main/java/frc/robot/commands/ConeVCube.java @@ -10,6 +10,7 @@ public class ConeVCube extends CommandBase { /** Creates a new ConeVCube. */ int gamePiece; + boolean finished; public ConeVCube(int gamePiece) { this.gamePiece = gamePiece; @@ -19,6 +20,7 @@ public ConeVCube(int gamePiece) { // Called when the command is initially scheduled. @Override public void initialize() { + finished = false; } // Called every time the scheduler runs while the command is scheduled. @@ -27,6 +29,8 @@ public void execute() { GlobalVariables.gamePiece = gamePiece; System.out.print(GlobalVariables.gamePiece); + finished=true; + } // Called once the command ends or is interrupted. @@ -36,6 +40,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return finished; } } diff --git a/src/main/java/frc/robot/commands/ConstantHold.java b/src/main/java/frc/robot/commands/ConstantHold.java new file mode 100644 index 0000000..06f3a91 --- /dev/null +++ b/src/main/java/frc/robot/commands/ConstantHold.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.Gripper; + +public class ConstantHold extends CommandBase { + Gripper gripper; + /** Creates a new ConstantHold. */ + public ConstantHold(Gripper gripper) { + this.gripper = gripper; + addRequirements(gripper); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(-0.075); + } + else{ + gripper.moveGripper(0.07); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java b/src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java new file mode 100644 index 0000000..e1feb78 --- /dev/null +++ b/src/main/java/frc/robot/commands/CustomSwerveTrajTrap.java @@ -0,0 +1,102 @@ +package frc.robot.commands; + +import frc.robot.Constants; +import frc.robot.subsystems.Swerve; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.CommandBase; + + +public class CustomSwerveTrajTrap extends CommandBase { + + private double rotation; + private Translation2d translation; + private boolean fieldRelative; + private boolean openLoop; + + private Swerve s_Swerve; + private Joystick controller; + private int translationAxis; + private int strafeAxis; + private int rotationAxis; + + ProfiledPIDController xPID; + ProfiledPIDController yPID; + PIDController rotPID; + + double goalX; + double goalY; + double goalRot; + + double maxSpeed; + double maxAngularVelocity; + + TrapezoidProfile.Constraints constraints; + + + /** + * Driver control + */ + public CustomSwerveTrajTrap(Swerve s_Swerve, double goalX, double goalY, double goalRot, double maxSpeed, double maxAngularVelocity) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + this.goalX = goalX; + this.goalY = goalY; + this.goalRot = goalRot; + this.maxSpeed = maxSpeed; + this.maxAngularVelocity = maxAngularVelocity; + + + constraints = new TrapezoidProfile.Constraints(maxSpeed, 1); + xPID = new ProfiledPIDController(/* 0.65 */2, 0.0, 0.02,constraints ); + yPID = new ProfiledPIDController(/* 0.65 */2, 0.0, 0.02, constraints); + rotPID = new PIDController(0.003, 0.0, 0); + + xPID.setTolerance(0.025); + yPID.setTolerance(0.025); + rotPID.setTolerance(1); + rotPID.enableContinuousInput(-180, 180); + + + + + + + } + @Override + public void initialize() { + + xPID.reset(s_Swerve.getPose().getX()); + yPID.reset(s_Swerve.getPose().getY()); + + xPID.setGoal(new TrapezoidProfile.State(goalX, 0)); + yPID.setGoal(new TrapezoidProfile.State(goalY, 0)); + + } + + + @Override + public void execute() { + double yAxis = xPID.calculate(s_Swerve.getPose().getX()); + double xAxis = yPID.calculate(s_Swerve.getPose().getY()); + double rAxis = rotPID.calculate(s_Swerve.getPose().getRotation().getDegrees(), goalRot); + + /* Deadbands */ + + + + translation = new Translation2d(yAxis, xAxis).times(11.5); + rotation = rAxis * maxAngularVelocity; + s_Swerve.drive(translation, rotation, true, false); + } + + @Override + public boolean isFinished() { + // return swerveControllerCommand.isFinished(); + return xPID.atSetpoint() && yPID.atSetpoint() && rotPID.atSetpoint(); + } + +} diff --git a/src/main/java/frc/robot/commands/CustomSwerveTrajectory.java b/src/main/java/frc/robot/commands/CustomSwerveTrajectory.java new file mode 100644 index 0000000..6e928ef --- /dev/null +++ b/src/main/java/frc/robot/commands/CustomSwerveTrajectory.java @@ -0,0 +1,82 @@ +package frc.robot.commands; + +import frc.robot.Constants; +import frc.robot.subsystems.Swerve; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + + +public class CustomSwerveTrajectory extends CommandBase { + + private double rotation; + private Translation2d translation; + private boolean fieldRelative; + private boolean openLoop; + + private Swerve s_Swerve; + private Joystick controller; + private int translationAxis; + private int strafeAxis; + private int rotationAxis; + + PIDController xPID; + PIDController yPID; + PIDController rotPID; + + double goalX; + double goalY; + double goalRot; + + double maxSpeed; + double maxAngularVelocity; + + + /** + * Driver control + */ + public CustomSwerveTrajectory(Swerve s_Swerve, double goalX, double goalY, double goalRot, double maxSpeed, double maxAngularVelocity) { + this.s_Swerve = s_Swerve; + addRequirements(s_Swerve); + + xPID = new PIDController(/* 0.65 */2, 0.0, 0.05); + yPID = new PIDController(/* 0.65 */2, 0.0, 0.05); + rotPID = new PIDController(0.003, 0.0, 0); + + xPID.setTolerance(0.025); + yPID.setTolerance(0.025); + rotPID.setTolerance(1); + rotPID.enableContinuousInput(-180, 180); + + + + this.goalX = goalX; + this.goalY = goalY; + this.goalRot = goalRot; + this.maxSpeed = maxSpeed; + this.maxAngularVelocity = maxAngularVelocity; + } + + @Override + public void execute() { + double yAxis = xPID.calculate(s_Swerve.getPose().getX(), goalX); + double xAxis = yPID.calculate(s_Swerve.getPose().getY(), goalY); + double rAxis = rotPID.calculate(s_Swerve.getPose().getRotation().getDegrees(), goalRot); + + /* Deadbands */ + + + + translation = new Translation2d(yAxis, xAxis).times(maxSpeed); + rotation = rAxis * maxAngularVelocity; + s_Swerve.drive(translation, rotation, true, false); + } + + @Override + public boolean isFinished() { + // return swerveControllerCommand.isFinished(); + return xPID.atSetpoint() && yPID.atSetpoint() && rotPID.atSetpoint(); + } + +} diff --git a/src/main/java/frc/robot/commands/FullArmPosition.java b/src/main/java/frc/robot/commands/FullArmPosition.java new file mode 100644 index 0000000..66147af --- /dev/null +++ b/src/main/java/frc/robot/commands/FullArmPosition.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class FullArmPosition extends CommandBase { + ArmSub armSub; + double armRotPos; + double armExtendPos; + double armJointPos; + boolean direction; + + + + /** Creates a new FullArmPosition. */ + public FullArmPosition(ArmSub armSub, double armRotPos, double armExtendPos, double armJointPos, boolean direction) { + this.armSub = armSub; + this.armRotPos = armRotPos; + this.armExtendPos = armExtendPos; + this.armJointPos = armJointPos; + this.direction = direction; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(direction){ + armSub.moveRotArmPosition(armRotPos); + if(armSub.getArmEncoderPosition() >= armRotPos*0.75){ + armSub.moveTelescopeArmPosition(armExtendPos); + armSub.moveGripperJointPosition(armJointPos); + } + } + else{ + armSub.moveTelescopeArmPosition(armExtendPos); + armSub.moveGripperJointPosition(armJointPos); + if(armSub.getTelescopePos() <= 30000){ + armSub.moveRotArmPosition(armRotPos); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java index 031da2c..602c474 100644 --- a/src/main/java/frc/robot/commands/GripperCommand.java +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -26,12 +26,15 @@ public void initialize() {} @Override public void execute() { gripper.moveGripper(percent); + // gripper.moveGripperPos(20); + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { gripper.moveGripper(0); + // gripper.moveGripperPos(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index fc84db7..ae41b4f 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -5,58 +5,68 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.GlobalVariables.ArmPositions; +import frc.robot.GlobalVariables; import frc.robot.subsystems.ArmSub; import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Gripper.GripperState; public class GroundIntake extends CommandBase { - /** Creates a new GroundIntake. */ ArmSub armSub; Gripper gripper; - double timer; - public GroundIntake(ArmSub armSub, Gripper gripper) { - this.gripper = gripper; this.armSub = armSub; - addRequirements(armSub, gripper); - + this.gripper = gripper; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. } + // Called when the command is initially scheduled. @Override - public void initialize() { - timer = 0; - } + public void initialize() {} + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - timer++; - armSub.armPresetPositions(ArmPositions.GROUND_PICKUP); - if (timer >= 20){ - gripper.setClaw(GripperState.OPEN); - gripper.moveGripper(1); + if(GlobalVariables.gamePiece == 0){ + armSub.moveGripperJointPosition(31000 * armSub.multiplier); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(-1); + } + } + + else{ + armSub.moveGripperJointPosition(22000*armSub.multiplier); + armSub.moveTelescopeArmPosition(8000); + armSub.moveRotArmPosition(1); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(0.8); + + } } } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - gripper.setClaw(GripperState.CONE); gripper.moveGripper(0); - System.out.println("END"); } + // Returns true when the command should end. @Override public boolean isFinished() { - if(gripper.colorSensorDistance() > 150){ - return true; - - } - else{ return false; - } } } diff --git a/src/main/java/frc/robot/commands/HoldArmPos.java b/src/main/java/frc/robot/commands/HoldArmPos.java index 1d9780e..1122ec3 100644 --- a/src/main/java/frc/robot/commands/HoldArmPos.java +++ b/src/main/java/frc/robot/commands/HoldArmPos.java @@ -33,7 +33,7 @@ public void initialize() { @Override public void execute() { timer ++; - armSub.armPresetPositions(armPos); + armSub.armRotPresetPositions(armPos); System.out.println("ARM COMMAND EXEC"); } diff --git a/src/main/java/frc/robot/commands/HoldArmPosAuto.java b/src/main/java/frc/robot/commands/HoldArmPosAuto.java index 05106bf..6a08152 100644 --- a/src/main/java/frc/robot/commands/HoldArmPosAuto.java +++ b/src/main/java/frc/robot/commands/HoldArmPosAuto.java @@ -34,7 +34,7 @@ public void initialize() { public void execute() { timer ++; double armpos = Constants.ArmPositions[GlobalVariables.gamePiece][GlobalVariables.height]; - armSub.moveArmPosition(armpos); + armSub.moveRotArmPosition(armpos); System.out.println("ARM COMMAND EXEC"); } diff --git a/src/main/java/frc/robot/commands/HoldJointPos.java b/src/main/java/frc/robot/commands/HoldJointPos.java new file mode 100644 index 0000000..e33c882 --- /dev/null +++ b/src/main/java/frc/robot/commands/HoldJointPos.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class HoldJointPos extends CommandBase { + /** Creates a new HoldJointPos. */ + ArmSub armSub; + double position; + + public HoldJointPos(ArmSub armSub, double position) { + this.armSub = armSub; + this.position = position; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armSub.moveGripperJointPosition(position); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/LockToGamePiece.java b/src/main/java/frc/robot/commands/LockRotation.java similarity index 66% rename from src/main/java/frc/robot/commands/LockToGamePiece.java rename to src/main/java/frc/robot/commands/LockRotation.java index cd5c92f..0e00c0a 100644 --- a/src/main/java/frc/robot/commands/LockToGamePiece.java +++ b/src/main/java/frc/robot/commands/LockRotation.java @@ -4,12 +4,12 @@ import frc.robot.GlobalVariables; import frc.robot.subsystems.Swerve; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; -public class LockToGamePiece extends CommandBase { +public class LockRotation extends CommandBase { private double rotation; private Translation2d translation; @@ -20,8 +20,12 @@ public class LockToGamePiece extends CommandBase { private Joystick controller; private int translationAxis; private int strafeAxis; + private int rotationAxis; + private double goal; + PIDController pidController; + + - private int pipeline; private double mapdouble(double x, double in_min, double in_max, double out_min, double out_max){ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } @@ -29,24 +33,34 @@ private double mapdouble(double x, double in_min, double in_max, double out_min, /** * Driver control */ - public LockToGamePiece(Swerve s_Swerve, Joystick controller, int translationAxis, int strafeAxis, boolean fieldRelative, boolean openLoop, int pipeline) { + public LockRotation(Swerve s_Swerve, Joystick controller, int translationAxis, int strafeAxis, int rotationAxis, boolean fieldRelative, boolean openLoop) { this.s_Swerve = s_Swerve; addRequirements(s_Swerve); this.controller = controller; this.translationAxis = translationAxis; this.strafeAxis = strafeAxis; + this.rotationAxis = rotationAxis; this.fieldRelative = fieldRelative; this.openLoop = openLoop; - this.pipeline = pipeline; + pidController = new PIDController(0.003 , 0.00, 0); + pidController.setTolerance(1); + pidController.enableContinuousInput(-180, 180); + + } @Override public void execute() { + if(GlobalVariables.robotDirection){ + goal = 180; + } + else{ + goal = 0; + } double yAxis = -controller.getRawAxis(translationAxis); double xAxis = controller.getRawAxis(strafeAxis); - double rAxis = s_Swerve.frontCamOffset(pipeline); - + double rAxis = pidController.calculate(s_Swerve.getYaw().getDegrees(), goal); /* Deadbands */ @@ -72,16 +86,16 @@ public void execute() { xAxis = 0; } - if (Math.abs(rAxis) > Constants.stickDeadband) { - if (rAxis > 0){ - rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); - } else { - rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); - } - } - else{ - rAxis = 0; - } + // if (Math.abs(rAxis) > Constants.stickDeadband) { + // if (rAxis > 0){ + // rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); + // } else { + // rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); + // } + // } + // else{ + // rAxis = 0; + // } translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); rotation = rAxis * Constants.Swerve.maxAngularVelocity; diff --git a/src/main/java/frc/robot/commands/AutoLockToGamePiece.java b/src/main/java/frc/robot/commands/LockToGrid.java similarity index 58% rename from src/main/java/frc/robot/commands/AutoLockToGamePiece.java rename to src/main/java/frc/robot/commands/LockToGrid.java index f8adb7e..90259e7 100644 --- a/src/main/java/frc/robot/commands/AutoLockToGamePiece.java +++ b/src/main/java/frc/robot/commands/LockToGrid.java @@ -4,12 +4,12 @@ import frc.robot.GlobalVariables; import frc.robot.subsystems.Swerve; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; -public class AutoLockToGamePiece extends CommandBase { +public class LockToGrid extends CommandBase { private double rotation; private Translation2d translation; @@ -17,11 +17,15 @@ public class AutoLockToGamePiece extends CommandBase { private boolean openLoop; private Swerve s_Swerve; - // private Joystick controller; + private Joystick controller; private int translationAxis; private int strafeAxis; + private int rotationAxis; + private double goal; + PIDController pidController; + + - private int pipeline; private double mapdouble(double x, double in_min, double in_max, double out_min, double out_max){ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } @@ -29,24 +33,34 @@ private double mapdouble(double x, double in_min, double in_max, double out_min, /** * Driver control */ - public AutoLockToGamePiece(Swerve s_Swerve, int translationAxis, int strafeAxis, boolean fieldRelative, boolean openLoop, int pipeline) { + public LockToGrid(Swerve s_Swerve, Joystick controller, int translationAxis, int strafeAxis, int rotationAxis, boolean fieldRelative, boolean openLoop) { this.s_Swerve = s_Swerve; addRequirements(s_Swerve); - // this.controller = controller; + this.controller = controller; this.translationAxis = translationAxis; this.strafeAxis = strafeAxis; + this.rotationAxis = rotationAxis; this.fieldRelative = fieldRelative; this.openLoop = openLoop; - this.pipeline = pipeline; + pidController = new PIDController(0.017 , 0.00, 0); + pidController.setTolerance(1); + pidController.enableContinuousInput(-180, 180); + + } @Override public void execute() { - double yAxis = 0.4; - double xAxis = 0; - double rAxis = s_Swerve.frontCamOffset(GlobalVariables.gamePiece); - + if(GlobalVariables.robotDirection){ + goal = 180; + } + else{ + goal = 0; + } + double yAxis = -controller.getRawAxis(translationAxis); + double xAxis = controller.getRawAxis(strafeAxis); /* -s_Swerve.getCamOffset() */; + double rAxis = pidController.calculate(s_Swerve.getYaw().getDegrees(), goal); /* Deadbands */ @@ -72,19 +86,19 @@ public void execute() { xAxis = 0; } - if (Math.abs(rAxis) > 0.01) { - if (rAxis > 0){ - rAxis = mapdouble(rAxis, 0.01, 1, 0, 1); - } else { - rAxis = mapdouble(rAxis, -0.01, -1, 0, -1); - } - } - else{ - rAxis = 0; - } + // if (Math.abs(rAxis) > Constants.stickDeadband) { + // if (rAxis > 0){ + // rAxis = mapdouble(rAxis, Constants.stickDeadband, 1, 0, 1); + // } else { + // rAxis = mapdouble(rAxis, -Constants.stickDeadband, -1, 0, -1); + // } + // } + // else{ + // rAxis = 0; + // } translation = new Translation2d(yAxis, xAxis).times(Constants.Swerve.maxSpeed); rotation = rAxis * Constants.Swerve.maxAngularVelocity; - s_Swerve.drive(translation, rotation, false, false); + s_Swerve.drive(translation, rotation, fieldRelative, openLoop); } } diff --git a/src/main/java/frc/robot/commands/ManualRollers.java b/src/main/java/frc/robot/commands/ManualRollers.java new file mode 100644 index 0000000..18d1620 --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualRollers.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import com.ctre.phoenix.CANifier.LEDChannel; +import com.ctre.phoenix.led.CANdle; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.CANdleSub; +import frc.robot.subsystems.Gripper; + +public class ManualRollers extends CommandBase { + /** Creates a new AdaptiveOuttake. */ + Gripper gripper; + CANdleSub candlesub; + public ManualRollers(Gripper gripper, CANdleSub candlesub) { + this.gripper = gripper; + this.candlesub = candlesub; + addRequirements(gripper, candlesub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + gripper.moveGripper(-1); + } + else{ + gripper.moveGripper(1); + } + + if(gripper.getGripperMotorCurrent() >= 100){ + candlesub.pickupLed(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/MoveJointTemp.java b/src/main/java/frc/robot/commands/MoveJointTemp.java new file mode 100644 index 0000000..2abbb8c --- /dev/null +++ b/src/main/java/frc/robot/commands/MoveJointTemp.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSub; + +public class MoveJointTemp extends CommandBase { + ArmSub armsub; + Joystick controller; + /** Creates a new MoveJointTemp. */ + public MoveJointTemp(ArmSub armSub, Joystick controller) { + this.armsub = armSub; + this.controller = controller; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + armsub.moveGripperJointPercentOutput(controller.getRawAxis(0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ShelfIntake.java b/src/main/java/frc/robot/commands/ShelfIntake.java index 82a0432..0183d1c 100644 --- a/src/main/java/frc/robot/commands/ShelfIntake.java +++ b/src/main/java/frc/robot/commands/ShelfIntake.java @@ -5,48 +5,73 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; import frc.robot.GlobalVariables.ArmPositions; import frc.robot.subsystems.ArmSub; import frc.robot.subsystems.Gripper; -import frc.robot.subsystems.Gripper.GripperState; public class ShelfIntake extends CommandBase { - - /** Creates a new GroundIntake. */ - Gripper gripper; - - public ShelfIntake( Gripper gripper) { - this.gripper = gripper; - addRequirements( gripper); + /** Creates a new AdaptiveArmMovement. */ + ArmSub armSub; + public ShelfIntake(ArmSub armSub) { + this.armSub = armSub; + + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. } + // Called when the command is initially scheduled. @Override - public void initialize() { - gripper.setClaw(GripperState.OPEN); - } + public void initialize() {} + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - gripper.moveGripper(0.8); + + + armSub.armRotPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); + armSub.jointRotPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); - } + if(armSub.getArmEncoderPosition() >= GlobalVariables.armRotGoal*0.75){ + armSub.armExtendPresetPositions(ArmPositions.SHELF_PICKUP_ADAPTIVE); + } + // if(GlobalVariables.gamePiece == 0){ + // if((gripper.getDistanceSensorDist() <= 12)){ + // gripper.moveGripper(0); + + // } + // else{ + // gripper.moveGripper(-1); + + // } + // } + // else{ + // if((gripper.getDistanceSensorDist() <= 12)){ + // gripper.moveGripper(0); + + // } + // else{ + // gripper.moveGripper(0.8); + + // } + } + + + + // } + + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - gripper.setClaw(GripperState.CONE); - gripper.moveGripper(0); - System.out.println("END"); + // gripper.moveGripper(0); + } + // Returns true when the command should end. @Override public boolean isFinished() { - if(gripper.colorSensorDistance() > 200){ - return true; - - } - else{ return false; - } } } diff --git a/src/main/java/frc/robot/commands/SingleSubstationIntake.java b/src/main/java/frc/robot/commands/SingleSubstationIntake.java new file mode 100644 index 0000000..5e3a78f --- /dev/null +++ b/src/main/java/frc/robot/commands/SingleSubstationIntake.java @@ -0,0 +1,72 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class SingleSubstationIntake extends CommandBase { + /** Creates a new GroundIntake. */ + ArmSub armSub; + Gripper gripper; + public SingleSubstationIntake(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(GlobalVariables.gamePiece == 0){ + armSub.moveGripperJointPosition(25000*armSub.multiplier); + armSub.moveTelescopeArmPosition(0); + armSub.moveRotArmPosition(40); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(-1); + + } + } + + else{ + armSub.moveGripperJointPosition(13000); + armSub.moveTelescopeArmPosition(0); + armSub.moveRotArmPosition(40); + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(0.8); + + } + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Test.java b/src/main/java/frc/robot/commands/Test.java index 6bfb59d..0b8007d 100644 --- a/src/main/java/frc/robot/commands/Test.java +++ b/src/main/java/frc/robot/commands/Test.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Gripper; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; public class Test extends CommandBase { Swerve swerve; diff --git a/src/main/java/frc/robot/commands/TestRed.java b/src/main/java/frc/robot/commands/TestRed.java index abb991a..9496424 100644 --- a/src/main/java/frc/robot/commands/TestRed.java +++ b/src/main/java/frc/robot/commands/TestRed.java @@ -19,7 +19,6 @@ import frc.robot.subsystems.Gripper; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Gripper.GripperState; public class TestRed extends CommandBase { Swerve swerve; diff --git a/src/main/java/frc/robot/commands/UprightConeIntake.java b/src/main/java/frc/robot/commands/UprightConeIntake.java new file mode 100644 index 0000000..e3c7123 --- /dev/null +++ b/src/main/java/frc/robot/commands/UprightConeIntake.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.GlobalVariables; +import frc.robot.subsystems.ArmSub; +import frc.robot.subsystems.Gripper; + +public class UprightConeIntake extends CommandBase { + /** Creates a new GroundIntake. */ + ArmSub armSub; + Gripper gripper; + public UprightConeIntake(ArmSub armSub, Gripper gripper) { + this.armSub = armSub; + this.gripper = gripper; + addRequirements(armSub); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + armSub.moveGripperJointPosition(47000 * armSub.multiplier); + armSub.moveTelescopeArmPosition(0); + armSub.moveRotArmPosition(47); + + if((gripper.getDistanceSensorDist() <= 12)){ + gripper.moveGripper(0); + + } + else{ + gripper.moveGripper(-1); + + } + + + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + gripper.moveGripper(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSub.java b/src/main/java/frc/robot/subsystems/ArmSub.java index e9fb27e..21d80f8 100644 --- a/src/main/java/frc/robot/subsystems/ArmSub.java +++ b/src/main/java/frc/robot/subsystems/ArmSub.java @@ -8,20 +8,15 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; -import com.revrobotics.ColorSensorV3; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.math.Conversions; import frc.robot.Constants; @@ -30,13 +25,28 @@ public class ArmSub extends SubsystemBase { public TalonFX leftArm; public TalonFX rightArm; - public PIDController pidLoop; - public TalonSRX encorderTalon; + + public TalonFX telescopeArm; + + public TalonFX gripperJointFalcon; + + + public PIDController armPID; + public PIDController jointPID; + public PIDController telescopePID; public CANCoder testCanCoder; - public TalonSRXFeedbackDevice encoder; - public double maxSpeedFor; - public double maxSpeedRev; + + public PIDController slowArmPID; + + private SupplyCurrentLimitConfiguration limit; + + public double rotMaxSpeedFor; + public double rotMaxSpeedRev; + + public double telMaxSpeedFor; + public double telMaxSpeedRev; + public double multiplier; /** Creates a new ArmSub. */ @@ -45,37 +55,53 @@ public ArmSub() { testCanCoder = new CANCoder(14, "Karen"); testCanCoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360); + testCanCoder.configSensorDirection(true); + testCanCoder.configMagnetOffset(150); + + multiplier = 0.677; + + + rotMaxSpeedFor = 1; + rotMaxSpeedRev = 0.6; - maxSpeedFor = 0.4; - maxSpeedRev = 0.65; + limit = new SupplyCurrentLimitConfiguration(true, 30, 30, 0); + - // pidLoop = new PIDController(0.006, 0.003, 0.001); - pidLoop = new PIDController(0.014, 0.002, 0.00); - pidLoop.setTolerance(0.5); - // pidLoop. + armPID = new PIDController(0.018, 0.000, 0.000); + armPID.setTolerance(0.1); + armPID.enableContinuousInput(0, 360); + + slowArmPID = new PIDController(0.007, 0.000, 0.000); + slowArmPID.setTolerance(0.1); + slowArmPID.enableContinuousInput(0, 360); + + + leftArm = new WPI_TalonFX(20, "Karen"); rightArm = new WPI_TalonFX(21, "Karen"); + telescopeArm = new WPI_TalonFX(22, "Karen"); + gripperJointFalcon = new WPI_TalonFX(40, "Karen"); leftArm.configFactoryDefault(); leftArm.setNeutralMode(NeutralMode.Brake); - leftArm.configPeakOutputForward(maxSpeedFor); - leftArm.configPeakOutputReverse(-maxSpeedRev); + leftArm.configPeakOutputForward(rotMaxSpeedFor); + leftArm.configPeakOutputReverse(-rotMaxSpeedRev); leftArm.config_kP(0, 0.05); leftArm.config_kI(0, 0.0); leftArm.config_kD(0, 0.0); leftArm.config_kF(0, 0.06); leftArm.setSensorPhase(true); - leftArm.setInverted(InvertType.None); + leftArm.setInverted(InvertType.InvertMotorOutput); leftArm.configRemoteFeedbackFilter(testCanCoder, 0); leftArm.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); rightArm.configFactoryDefault(); - rightArm.configPeakOutputForward(maxSpeedFor); - rightArm.configPeakOutputReverse(-maxSpeedRev); + rightArm.configPeakOutputForward(rotMaxSpeedFor); + rightArm.configPeakOutputReverse(-rotMaxSpeedRev); rightArm.setSensorPhase(true); rightArm.setInverted(InvertType.FollowMaster); rightArm.config_kP(0, 0.05); @@ -84,132 +110,560 @@ public ArmSub() { rightArm.config_kF(0, 0.06); rightArm.setNeutralMode(NeutralMode.Brake); rightArm.follow(leftArm); + + telMaxSpeedFor = 1; + telMaxSpeedRev = 1; + + telescopeArm.configFactoryDefault(); + telescopeArm.configPeakOutputForward(telMaxSpeedFor); + telescopeArm.configPeakOutputReverse(-telMaxSpeedRev); + telescopeArm.setSensorPhase(true); + telescopeArm.setInverted(InvertType.None); + telescopeArm.config_kP(0, 0.7); + telescopeArm.config_kI(0, 0.0); + telescopeArm.config_kD(0, 0.0); + telescopeArm.config_kF(0, 0.0); + telescopeArm.setNeutralMode(NeutralMode.Brake); + telescopeArm.configNeutralDeadband(0.1); + telescopeArm.configMotionAcceleration(45000); + telescopeArm.configMotionCruiseVelocity(300000); + telescopeArm.setSelectedSensorPosition(0); + telescopeArm.configSupplyCurrentLimit(limit); + + gripperJointFalcon.configFactoryDefault(); + gripperJointFalcon.configPeakOutputForward(telMaxSpeedFor); + gripperJointFalcon.configPeakOutputReverse(-telMaxSpeedRev); + gripperJointFalcon.setSensorPhase(true); + gripperJointFalcon.setInverted(InvertType.InvertMotorOutput); + gripperJointFalcon.config_kP(0, 0.6); + gripperJointFalcon.config_kI(0, 0.0); + gripperJointFalcon.config_kD(0, 0.0); + gripperJointFalcon.config_kF(0, 0.0); + gripperJointFalcon.setNeutralMode(NeutralMode.Brake); + gripperJointFalcon.configNeutralDeadband(0.001); + gripperJointFalcon.configMotionAcceleration(30000); + gripperJointFalcon.configMotionCruiseVelocity(40000); + gripperJointFalcon.setSelectedSensorPosition(0); + gripperJointFalcon.configAllowableClosedloopError(0, 10); + + gripperJointFalcon.configSupplyCurrentLimit(limit); + // gripperJointFalcon.setSensorPhase(false); + + } - + public void ArmBrakeMode(NeutralMode mode){ + telescopeArm.setNeutralMode(mode); + rightArm.setNeutralMode(mode); + leftArm.setNeutralMode(mode); + } + + public void moveRotArmPosition(double degrees){ + leftArm.set(ControlMode.PercentOutput, armPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); + + } + + public void moveRotArmPositionSlow(double degrees){ + leftArm.set(ControlMode.PercentOutput, slowArmPID.calculate(testCanCoder.getAbsolutePosition(), degrees)); + } + + /** * Moves the arm by percent output. * @param percent 0.0 - 1.0. */ - public void moveArmPercentOutput(double percent){ + public void moveRotArmPercentOutput(double percent){ leftArm.set(TalonFXControlMode.PercentOutput, percent); } - /** - * Sets the encoders within the arm Falcons to the current CANCoder readout. - * - */ - private void resetToAbsolute(){ - double absolutePosition = Conversions.degreesToFalcon(testCanCoder.getAbsolutePosition(), (56)); - leftArm.setSelectedSensorPosition(absolutePosition); + +/** + * Moves the arm telescope using percent control. + * @param percent + */ +public void moveTelescopeArmPercentOutput(double percent){ + telescopeArm.set(TalonFXControlMode.PercentOutput, percent); +} + +public void moveTelescopeArmPosition(double position){ + // telescopeArm.set(TalonFXControlMode.Position, position); + telescopeArm.set(TalonFXControlMode.MotionMagic, position); +} + +public void moveGripperJointPosition(double position){ + gripperJointFalcon.set(TalonFXControlMode.MotionMagic, position); +} + + + +public void moveGripperJointPercentOutput(double percent ){ + // gripperJointNeo.set(percent); + gripperJointFalcon.set(TalonFXControlMode.PercentOutput, percent); + +} + +public double getTelescopePos(){ + return telescopeArm.getSelectedSensorPosition(); +} +public void homeTelescopePosition(){ + telescopeArm.setSelectedSensorPosition(0); } + +public void homeRotArmPos(){ + // TODO THING + leftArm.setSelectedSensorPosition(testCanCoder.getPosition() /30); +} + +public void homeGripperJointPos(){ + // gripperJointNeo.getEncoder().setPosition(0); + gripperJointFalcon.setSelectedSensorPosition(0); +} + /** * Moves the arm to a dedicated degree. * @param position Desired arm position in degrees */ - public void moveArmPosition(double position){ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(testCanCoder.getAbsolutePosition(), position)); - } + /** * Moves the arm to a dedicated preset positions * @param position The dedicated position enum, current options are GROUND_PICKUP, STOWED, GROUND_SCORE, MID_SCORE, HIGH_SCORE, and SHELF_PICKUP */ - public void armPresetPositions(GlobalVariables.ArmPositions position){ + public void armRotPresetPositions(GlobalVariables.ArmPositions position){ switch(position){ - case GROUND_PICKUP: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.grndIntakePosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.grndIntakePosValue)); + case GROUND_PICKUP_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotForConePickup); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForConePickup; + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotForCubePickup); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForCubePickup; + + } + break; + + case STOWED_ADAPTIVE: + if(GlobalVariables.robotDirection){ + moveRotArmPositionSlow(Constants.ArmRotationValues.armRotStow); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotStow); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; + } break; - case STOWED: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.stowedPosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.stowedPosValue)); + case GROUND_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotForLowCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForLowCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotForLowCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForLowCube; + + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevLowCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevLowCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevLowCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevLowCube; + } + } break; - case GROUND_SCORE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.grndScorePosValueCone)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.grndScorePosValue)); + case SHELF_PICKUP_ADAPTIVE: + + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotForShelfCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForShelfCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotForShelfCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForShelfCube; + + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevShelfCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevShelfCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevShelfCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevShelfCube; + + } + } + break; + case MID_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotForMidCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForMidCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotForMidCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForMidCube; + + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevMidCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevMidCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevMidCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevMidCube; + + } + } break; - case MID_SCORE_CONE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCone)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.midScorePosValue)); + case HIGH_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition(Constants.ArmRotationValues.armRotForHighCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForHighCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotForHighCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotForHighCube; + + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveRotArmPosition( Constants.ArmRotationValues.armRotRevHighCone); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevHighCone; + + } + else{ + moveRotArmPosition(Constants.ArmRotationValues.armRotRevHighCube); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotRevHighCube; + + } + } + break; + case FRAME_PERIMETER: + moveRotArmPosition(Constants.ArmRotationValues.framePerimeter); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.framePerimeter; break; - case HIGH_SCORE_CONE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCone)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); + default: + moveRotArmPosition(Constants.ArmRotationValues.armRotStow); + GlobalVariables.armRotGoal = Constants.ArmRotationValues.armRotStow; break; - case MID_SCORE_CUBE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCube)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.midScorePosValue)); + } + } + + public void armExtendPresetPositions(GlobalVariables.ArmPositions position){ + switch(position){ + case GROUND_PICKUP_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendConePickup); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendConePickup; + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendCubePickup); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendCubePickup; + + + } break; - case HIGH_SCORE_CUBE: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCube)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); + case STOWED_ADAPTIVE: + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendStow); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendStow; break; - case SHELF_PICKUP: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.shelfIntakePosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + case GROUND_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForLowCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForLowCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForLowCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForLowCube; + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevLowCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevLowCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevLowCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevLowCube; + + + } + } + break; - case MID_SCORE_ADAPTIVE: - if(GlobalVariables.gamePiece == 0){ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCone)); + case SHELF_PICKUP_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForShelfCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForShelfCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForShelfCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForShelfCube; + + } } else{ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.midScorePosValueCube)); + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevShelfCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevShelfCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevShelfCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevShelfCube; + + + } + } + break; + + case MID_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForMidCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForMidCone; + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForMidCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForMidCube; + + } } - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + else{ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevMidCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevMidCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevMidCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevMidCube; + + } + } + break; + case HIGH_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForHighCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForHighCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendForHighCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendForHighCube; + + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevHighCone); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevHighCone; + + } + else{ + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendRevHighCube); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendRevHighCube; + + + } + } + + break; + + case FRAME_PERIMETER: + moveTelescopeArmPosition(Constants.ArmExtendValues.framePerimeter); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.framePerimeter; + break; + + default: + moveTelescopeArmPosition(Constants.ArmExtendValues.armExtendStow); + GlobalVariables.armExtendGoal = Constants.ArmExtendValues.armExtendStow; + + + break; + } + } + + public void jointRotPresetPositions(GlobalVariables.ArmPositions position){ + switch(position){ + + case GROUND_PICKUP_ADAPTIVE: + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotConePickup * multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotCubePickup* multiplier); + + } + break; + + case STOWED_ADAPTIVE: if(GlobalVariables.gamePiece == 0){ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCone)); + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube* multiplier); + + } + break; + + case GROUND_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForLowCube* multiplier); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevLowCube* multiplier); + + } + } + + break; + + case SHELF_PICKUP_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForShelfCube* multiplier); + } } else{ - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.highScorePosValueCube)); + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevShelfCube* multiplier); + } } - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + break; + case MID_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForMidCube* multiplier); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevMidCube* multiplier); + + } + } + break; - case CONE_UPRIGHT: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.coneUprightPosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.highScorePosValue)); + case HIGH_SCORE_ADAPTIVE: + if(GlobalVariables.robotDirection){ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotForHighCube* multiplier); + } + } + else{ + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotRevHighCube* multiplier); + } + } + break; + case FRAME_PERIMETER: + moveGripperJointPosition(Constants.JointRotationValues.framePerimeter * multiplier); + break; default: - leftArm.set(ControlMode.PercentOutput ,pidLoop.calculate(getArmPosition(), Constants.shelfIntakePosValue)); - // System.out.println("CancoderPos" + getArmPosition() + "PID Loop Output"+pidLoop.calculate(testCanCoder.getAbsolutePosition(), Constants.shelfIntakePosValue)); + if(GlobalVariables.gamePiece == 0){ + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCone* multiplier); + } + else{ + moveGripperJointPosition(Constants.JointRotationValues.JointRotStowCube* multiplier); + } break; } } + /** * Returns current arm position * @return CANcoder arm position in degrees. */ - public double getArmPosition(){ + public double getArmEncoderPosition(){ return testCanCoder.getAbsolutePosition(); } + + public double getRotArmPos(){ + // TODO make this gearbox to degrees + return leftArm.getSelectedSensorPosition() /3; + } /** * Returns current arm motor position. * @return Current arm falcon position unfiltered. @@ -227,14 +681,23 @@ public double getArmMotorPos(){ return test; } + public double getGripperJointPos(){ + // return jointEncoder.getPosition(); + return gripperJointFalcon.getSelectedSensorPosition(); + } + @Override public void periodic() { SmartDashboard.putNumber("PID OUTPUT ", leftArm.getMotorOutputPercent()); - SmartDashboard.putNumber("PID ERROR ", pidLoop.getPositionError()); - SmartDashboard.putNumber("Arm Encoder Position", getArmPosition() ); + SmartDashboard.putData("PID ERROR ", armPID); + SmartDashboard.putNumber("Arm Encoder Position", getArmEncoderPosition() ); SmartDashboard.putNumber("Arm Motor Position", getArmMotorPos()); - SmartDashboard.putNumber("Arm Motor 1 Current", leftArm.getStatorCurrent()); - SmartDashboard.putNumber("Arm Motor 2 Current", rightArm.getStatorCurrent()); + SmartDashboard.putNumber("Arm Motor Actual Speed", leftArm.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Arm Motor Set Speed", leftArm.getMotorOutputPercent()); + SmartDashboard.putNumber("Telescope Position", telescopeArm.getSelectedSensorPosition()); + SmartDashboard.putNumber("Telescope Speed?", telescopeArm.getSupplyCurrent()); + SmartDashboard.putNumber("Joint Position ", getGripperJointPos() ); + SmartDashboard.putNumber("Joint Speed ", gripperJointFalcon.getSelectedSensorVelocity()); } } diff --git a/src/main/java/frc/robot/subsystems/CANdleSub.java b/src/main/java/frc/robot/subsystems/CANdleSub.java index 001594f..f7c5ce6 100644 --- a/src/main/java/frc/robot/subsystems/CANdleSub.java +++ b/src/main/java/frc/robot/subsystems/CANdleSub.java @@ -14,7 +14,7 @@ public class CANdleSub extends SubsystemBase { private final CANdle m_candle = new CANdle(Constants.CANdle.id, "rio"); - private final int numLed = 48 + 32 + 32 + 48; + private final int numLed = /* 48 + 32 + 32 + 48 */200; private final int ledOffset = 8; public CANdleSub() { @@ -30,17 +30,21 @@ public CANdleSub() { } public void disabledLed(){ - m_candle.animate(new SingleFadeAnimation(252, 40, 101, 0, 0.6, numLed, ledOffset)); + m_candle.animate(new SingleFadeAnimation(200, 0, 0, 0, 0.6, numLed, ledOffset)); } public void enabledLed(){ m_candle.animate(new SingleFadeAnimation(230, 10, 10, 0, 0.7, numLed, ledOffset)); } + public void pickupLed(){ + m_candle.setLEDs(0, numLed, 0, 0, ledOffset, numLed); + } + public void setColor(boolean cone) { m_candle.clearAnimation(0); if (cone) { - m_candle.setLEDs(252, 186, 3, 0, ledOffset, numLed); + m_candle.setLEDs(255, 45, 0, 0, ledOffset, numLed); return; } m_candle.setLEDs(111, 3, 252, 0, ledOffset, numLed); diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index bfe4b3e..c7ec640 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -4,111 +4,59 @@ package frc.robot.subsystems; -import com.revrobotics.CANSparkMax; -import com.revrobotics.ColorSensorV3; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.PneumaticsControlModule; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import com.revrobotics.Rev2mDistanceSensor; + +import com.revrobotics.Rev2mDistanceSensor.Port; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Gripper extends SubsystemBase { - CANSparkMax gripperMotor; - DoubleSolenoid cubeSolenoid; - Solenoid coneSolenoid; - public PneumaticsControlModule module; - Compressor compressor; - public final I2C.Port i2cPort = I2C.Port.kMXP; + public TalonFX gripperFalcon; + private SupplyCurrentLimitConfiguration limit; - public final ColorSensorV3 colorSensor = new ColorSensorV3(i2cPort); -; - public Color detectedColor; - public enum GripperState{ - OPEN, CONE, CUBE - } - public GripperState gripperState = GripperState.OPEN; + private Rev2mDistanceSensor distanceSensor; + /** Creates a new Gripper. */ public Gripper() { - gripperMotor = new CANSparkMax(42, MotorType.kBrushless); - gripperMotor.restoreFactoryDefaults(); - gripperMotor.setIdleMode(IdleMode.kBrake); + gripperFalcon = new TalonFX(35, "Karen"); + gripperFalcon.configFactoryDefault(); + gripperFalcon.setNeutralMode(NeutralMode.Brake); - compressor = new Compressor(PneumaticsModuleType.REVPH); - compressor.enableDigital(); - cubeSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, 15, 14); - // coneSolenoid = new DoubleS/olenoid(PneumaticsModuleType.REVPH, 1, 3); - coneSolenoid = new Solenoid(PneumaticsModuleType.REVPH, 1); + limit = new SupplyCurrentLimitConfiguration(true, 50, 50, 0.5); + // gripperFalcon.configSupplyCurrentLimit(limit); - module = new PneumaticsControlModule(1); - module.clearAllStickyFaults(); - - detectedColor = colorSensor.getColor(); - } - public void getColorSensor(){ - detectedColor = colorSensor.getColor(); - } - public double getRed(){ - getColorSensor(); - return detectedColor.red; - } + distanceSensor = new Rev2mDistanceSensor(Port.kMXP); + distanceSensor.setAutomaticMode(true); - public double getBlue(){ - getColorSensor(); - return detectedColor.blue; } - public double getGreen(){ - getColorSensor(); - return detectedColor.green; + public double getDistanceSensorDist(){ + return distanceSensor.getRange(); } - public double colorSensorDistance(){ - return colorSensor.getProximity(); + public double getGripperMotorCurrent(){ + return gripperFalcon.getSupplyCurrent(); } - public void moveGripper(double percent ){ - gripperMotor.set(percent); - } - - public void setClaw(GripperState state){ - switch(state){ - case OPEN: - cubeSolenoid.set(Value.kReverse); - // coneSolenoid.set(false); - break; - - case CONE: - cubeSolenoid.set(Value.kForward); - // coneSolenoid.set(false); - break; - - case CUBE: - // cubeSolenoid.set(Value.kOff); - // coneSolenoid.set(true); - break; + gripperFalcon.set(TalonFXControlMode.PercentOutput, percent); } -} - @Override public void periodic() { - SmartDashboard.putNumber("RED", colorSensor.getRed()); - SmartDashboard.putNumber("BLUE", colorSensor.getBlue()); - SmartDashboard.putNumber("GREEN", colorSensor.getGreen()); - SmartDashboard.putNumber("Distance", colorSensor.getProximity()); + SmartDashboard.putNumber("Distance Sensor", getDistanceSensorDist()); + SmartDashboard.putNumber("GripperCurrent", gripperFalcon.getSupplyCurrent()); + } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 2a1e990..49920d3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,8 +1,6 @@ package frc.robot.subsystems; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; import com.kauailabs.navx.frc.AHRS; import frc.robot.SwerveModule; @@ -13,7 +11,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -22,6 +19,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -34,6 +33,7 @@ public class Swerve extends SubsystemBase { public PhotonCameraWrapper pcw; public AHRS gyro; public ProfiledPIDController rotatePID; + public PIDController aimPID; private SwerveModule m_frontLeft = new SwerveModule(0, Constants.Swerve.Mod0.constants); private SwerveModule m_frontRight = new SwerveModule(1, Constants.Swerve.Mod1.constants); @@ -77,6 +77,8 @@ public Swerve() { // zeroGyro(); SmartDashboard.putData("Field", m_fieldSim); + aimPID = new PIDController(0.05, 0, 0); + aimPID.setTolerance(0.5); rotatePID = new ProfiledPIDController( 0.010, @@ -94,7 +96,7 @@ public Swerve() { swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getYaw(), mSwerveModulePositions); - + m_poseEstimator = new SwerveDrivePoseEstimator( Constants.Swerve.swerveKinematics, getYaw(), new SwerveModulePosition[] { m_frontRight.getPosition(), m_frontLeft.getPosition(), @@ -116,24 +118,24 @@ public void kyslol(){ public void updateOdometry() { - Optional result = pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition()); + // Optional result = pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition()); m_poseEstimator.update(getYaw(), new SwerveModulePosition[] { m_frontRight.getPosition(), m_frontLeft.getPosition(), m_backRight.getPosition(), m_backLeft.getPosition()}); - if (result.isPresent()) { + // if (result.isPresent()) { - EstimatedRobotPose camPose = result.get(); + // EstimatedRobotPose camPose = result.get(); - m_poseEstimator.addVisionMeasurement( - camPose.estimatedPose.toPose2d(), camPose.timestampSeconds); + // m_poseEstimator.addVisionMeasurement( + // camPose.estimatedPose.toPose2d(), camPose.timestampSeconds); - m_fieldSim.getObject("Cam Est Pos").setPose( m_poseEstimator.getEstimatedPosition() ); - } else { - m_fieldSim.getObject("Cam Est Pos").setPose(m_poseEstimator.getEstimatedPosition()); - } + // m_fieldSim.getObject("Cam Est Pos").setPose( m_poseEstimator.getEstimatedPosition() ); + // } else { + // // m_fieldSim.getObject("Cam Est Pos").setPose(m_poseEstimator.getEstimatedPosition()); + // } m_poseEstimator.update(getYaw(), new SwerveModulePosition[] { m_frontRight.getPosition(), @@ -141,16 +143,35 @@ public void updateOdometry() { m_backRight.getPosition(), m_backLeft.getPosition()}); - m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); + m_fieldSim.setRobotPose(getPose()); SmartDashboard.putNumber("lmao X", m_poseEstimator.getEstimatedPosition().getX()); SmartDashboard.putNumber("lmao Y", m_poseEstimator.getEstimatedPosition().getY()); - SmartDashboard.putBoolean("AprilTag in View", result.isPresent()); + // SmartDashboard.putBoolean("AprilTag in View", result.isPresent()); // m_fieldSim.setRobotPose(getPose()); // m_fieldSim.getObject("Actual Pos").setPose(getPose()); // m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition()); } + public void updateRobotDirection(){ + // if (DriverStation.getAlliance() == Alliance.Blue){ + if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ + GlobalVariables.robotDirection = true; + } + else{ + GlobalVariables.robotDirection = false; + // } + } + // else{ + // if(gyro.getYaw()>=90 || gyro.getYaw() <= -90){ + // GlobalVariables.robotDirection = false; + // } + // else{ + // GlobalVariables.robotDirection = true; + // } + // } + } + public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { SwerveModuleState[] swerveModuleStates = @@ -195,7 +216,12 @@ public Pose2d getGlobalPose(){ public void resetOdometry(Pose2d pose) { // swerveOdometry.resetPosition(getYaw(), mSwerveModulePositions, pose); - swerveOdometry.resetPosition(getYaw(), mSwerveModulePositions, new Pose2d()); + swerveOdometry.resetPosition(getYaw(), new SwerveModulePosition[] { + m_frontRight.getPosition(), + m_frontLeft.getPosition(), + m_backRight.getPosition(), + m_backLeft.getPosition() + }, pose); } public void normalizeOdometry(){ @@ -244,49 +270,64 @@ public Rotation2d getYaw() { } public double getPitch(){ - return gyro.getRoll(); + return gyro.getPitch(); } + public double getCamOffset(){ + pcw.cameraPipelines(GlobalVariables.gamePiece); + double headingError; + if(GlobalVariables.robotDirection){ + headingError = pcw.getFrontCamOffset(); + return aimPID.calculate(headingError, 30); + } + else{ + headingError = pcw.getRearCamOffset(); + return aimPID.calculate(headingError, 9); - - public double frontCamOffset(int pipeline){ - pcw.frontCamPipeline(GlobalVariables.gamePiece); - - double headingError = pcw.getYOffset(); - /* double steering_adjust; - steering_adjust = 0; - if (headingError > 0.05) - { - steering_adjust = 0.09; } - else if (headingError < -0.05) - { - steering_adjust = -0.09; - } */ - return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); - } + } - - public double AutoFrontCamOffset(int pipeline){ - pcw.frontCamPipeline(GlobalVariables.gamePiece); - double headingError = pcw.AutoGetYOffset(); - /* double steering_adjust; - steering_adjust = 0; - if (headingError > 0.05) - { - steering_adjust = 0.09; - } - else if (headingError < -0.05) - { - steering_adjust = -0.09; - } */ - + // public double frontCamOffset(int pipeline){ + // pcw.frontCamPipeline(GlobalVariables.gamePiece); + + // double headingError = pcw.getYOffset(); + // /* double steering_adjust; + // steering_adjust = 0; + // if (headingError > 0.05) + // { + // steering_adjust = 0.09; + // } + // else if (headingError < -0.05) + // { + // steering_adjust = -0.09; + // } */ + + + // return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); + // } - return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); - } + + // public double AutoFrontCamOffset(int pipeline){ + // pcw.frontCamPipeline(GlobalVariables.gamePiece); + + // double headingError = pcw.AutoGetYOffset(); + // /* double steering_adjust; + // steering_adjust = 0; + // if (headingError > 0.05) + // { + // steering_adjust = 0.09; + // } + // else if (headingError < -0.05) + // { + // steering_adjust = -0.09; + // } */ + + + // return (rotatePID.calculate(headingError, 0)/* +steering_adjust */); + // } @@ -295,7 +336,9 @@ else if (headingError < -0.05) @Override public void periodic(){ + // updateRobotDirection(); // pcw.frontCamPipeline(2); + // pcw.cameraPipelines(GlobalVariables.gamePiece); AprilTagGrid.setDefaultOption("Left Tag", gridTag1); AprilTagGrid.addOption("Mid Tag", gridTag2); @@ -317,12 +360,17 @@ public void periodic(){ SmartDashboard.putNumber("Odometry X", getPose().getX()); SmartDashboard.putNumber("Odometry Y", getPose().getY()); - SmartDashboard.putNumber("CONE OFFSET ", frontCamOffset(1)); + // SmartDashboard.putNumber("CONE OFFSET ", frontCamOffset(1)); SmartDashboard.putNumber("Gyro Yaw ", gyro.getYaw()); + SmartDashboard.putNumber("Gyro Pitch ", gyro.getPitch()); + SmartDashboard.putNumber("Gyro Roll ", gyro.getRoll()); + SmartDashboard.putNumber("AprilTag Yaw ", getAprilTagEstPosition().getRotation().getDegrees()); + SmartDashboard.putBoolean("robot direction ", GlobalVariables.robotDirection); + SmartDashboard.putNumber("Rear Cam offset ", pcw.getRearCamOffset()); kyslol(); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..69fddcd --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,35 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2023.4.3", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2023.4.3" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2023.4.3", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 0000000..ecf0636 --- /dev/null +++ b/vendordeps/REV2mDistanceSensor.json @@ -0,0 +1,55 @@ +{ + "fileName": "REV2mDistanceSensor.json", + "name": "REV2mDistanceSensor", + "version": "0.4.0", + "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", + "mavenUrls": [ + "https://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "https://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-java", + "version": "0.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-cpp", + "version": "0.4.0", + "libName": "libDistanceSensor", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "0.4.0", + "libName": "libDistanceSensorDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file