Compare commits
19 Commits
a19285cb0b
...
faster_ele
| Author | SHA1 | Date | |
|---|---|---|---|
| ba7e8d59ad | |||
|
626b92b769 | ||
|
|
42d47d6075 | ||
|
|
68da3c630c | ||
| c9316cebc3 | |||
| d312e125cd | |||
| 4386de4d4d | |||
| cca7d68766 | |||
| a8a597985f | |||
| 060b39669f | |||
|
|
4ada896603 | ||
|
|
dd26ff6de4 | ||
|
|
0589463c4e | ||
| 339bf642a1 | |||
| c75554dfc5 | |||
| e98b3a585e | |||
| 83db16794f | |||
| 3dcbac25cc | |||
| a391cc7910 |
@@ -27,6 +27,12 @@
|
||||
"data": {
|
||||
"pathName": "H Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
74
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto
Normal file
74
src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto
Normal file
@@ -0,0 +1,74 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Start to H"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Coral L4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "H Backup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Pickup Algae L2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HG Algae"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "HG to Barge"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shoot Algae"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "HP Pickup"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Post-Barge Backup"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -8,20 +8,20 @@
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.649657534252619,
|
||||
"y": 6.474186643842743
|
||||
"x": 4.843032786885245,
|
||||
"y": 6.30266393442623
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1987704918032787,
|
||||
"y": 7.189754098360656
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.055234532899169,
|
||||
"y": 6.243135947675724
|
||||
"x": 2.287270519874242,
|
||||
"y": 6.774371912194027
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
|
||||
@@ -46,7 +46,7 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 3.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
"x": 5.758260140458621,
|
||||
"y": 4.193633481772718
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.053790983606557,
|
||||
"y": 4.312704918032787
|
||||
"x": 6.112051124065178,
|
||||
"y": 4.206338399805505
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "H"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.389446721311475,
|
||||
"y": 4.3
|
||||
"x": 6.5,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.1394487099079695,
|
||||
"y": 4.300997143065429
|
||||
"x": 6.250001988596495,
|
||||
"y": 4.022982203795822
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "Behind HG Face"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -34,7 +34,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAcceleration": 0.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -45,10 +45,10 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/HG Algae.path
Normal file
54
src/main/deploy/pathplanner/paths/HG Algae.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.5,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.224227344787433,
|
||||
"y": 4.009974315068493
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Behind HG Face"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.872671261177789,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.125859753477119,
|
||||
"y": 4.004837296105254
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "HG Algae"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
71
src/main/deploy/pathplanner/paths/HG to Barge.path
Normal file
71
src/main/deploy/pathplanner/paths/HG to Barge.path
Normal file
@@ -0,0 +1,71 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.872671261177789,
|
||||
"y": 4.021985060730393
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 7.829105562465259,
|
||||
"y": 3.8676166530198106
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HG Algae"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.032020547945206,
|
||||
"y": 4.761258561643835
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.262336691900294,
|
||||
"y": 4.359747737103425
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Pre-Barge"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.17234468937875755,
|
||||
"rotationDegrees": 180.0
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Processor Position",
|
||||
"waypointRelativePos": 0.188095238095238,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Processor Position"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 300.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1987704918032787,
|
||||
"y": 7.189754098360656
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.490979395912868,
|
||||
"y": 6.964368824388053
|
||||
"x": 2.0019467213114757,
|
||||
"y": 6.434528688524591
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.031967213114754
|
||||
"x": 3.609900518622585,
|
||||
"y": 5.005924534374863
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.304747501684258,
|
||||
"y": 5.557866185717494
|
||||
"x": 3.2943611350609414,
|
||||
"y": 5.531823506977602
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -46,7 +46,7 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAcceleration": 1.25,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -62,5 +62,5 @@
|
||||
"velocity": 0,
|
||||
"rotation": -53.97262661489646
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,13 +3,13 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1987704918032787,
|
||||
"y": 7.189754098360656
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.7142418032786886,
|
||||
"y": 6.638319672131147
|
||||
"x": 1.9266540815180775,
|
||||
"y": 6.744320542331013
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "HP Left Position"
|
||||
@@ -20,8 +20,8 @@
|
||||
"y": 5.211782786885246
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 3.2246926229508195,
|
||||
"y": 6.230737704918033
|
||||
"x": 2.975085616438356,
|
||||
"y": 6.023416095890411
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -45,8 +45,8 @@
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 1.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -62,5 +62,5 @@
|
||||
"velocity": 0,
|
||||
"rotation": -53.97262661489646
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6202868852459016,
|
||||
"y": 5.031967213114754
|
||||
"x": 3.609900518622585,
|
||||
"y": 5.005924534374863
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.4877871502451065,
|
||||
"y": 5.243966789116026
|
||||
"x": 3.47740078362179,
|
||||
"y": 5.217924110376135
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "L"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.1987704918032787,
|
||||
"y": 7.189754098360656
|
||||
"x": 1.1268442622950818,
|
||||
"y": 7.201741803278688
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.4025614754098363,
|
||||
"y": 6.914036885245903
|
||||
"x": 1.3306352459016395,
|
||||
"y": 6.926024590163935
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -34,7 +34,7 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 5.0,
|
||||
"maxAcceleration": 2.5,
|
||||
"maxAcceleration": 3.5,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
|
||||
54
src/main/deploy/pathplanner/paths/New Path.path
Normal file
54
src/main/deploy/pathplanner/paths/New Path.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 7.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.0,
|
||||
"y": 7.0
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.756421232876712,
|
||||
"y": 5.227054794520548
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.756421232876712,
|
||||
"y": 5.227054794520548
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": -59.18537788806707
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Left Paths",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
54
src/main/deploy/pathplanner/paths/Post-Barge Backup.path
Normal file
54
src/main/deploy/pathplanner/paths/Post-Barge Backup.path
Normal file
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.032020547945206,
|
||||
"y": 4.761258561643835
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 6.71311475409836,
|
||||
"y": 4.9480532786885245
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Pre-Barge"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.933913934426228,
|
||||
"y": 5.247745901639345
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.641188524590164,
|
||||
"y": 5.043954918032786
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -46,7 +46,7 @@
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAcceleration": 1.25,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -62,5 +62,5 @@
|
||||
"velocity": 0,
|
||||
"rotation": -90.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -16,12 +16,12 @@
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 5.7,
|
||||
"y": 4.3
|
||||
"x": 5.758260140458621,
|
||||
"y": 4.193633481772718
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 6.347336065573771,
|
||||
"y": 4.2640368852459005
|
||||
"x": 6.405596206032391,
|
||||
"y": 4.157670367018619
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -31,10 +31,22 @@
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Lift L4",
|
||||
"waypointRelativePos": 0.10238095238095252,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": {
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Lift L4"
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.5,
|
||||
"maxAcceleration": 1.75,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 400.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -45,10 +57,10 @@
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": null,
|
||||
"folder": "Center",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,7 +3,8 @@
|
||||
"robotLength": 0.8763,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [
|
||||
"Left Paths"
|
||||
"Left Paths",
|
||||
"Center"
|
||||
],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 3.5,
|
||||
|
||||
@@ -111,9 +111,9 @@ public class RobotContainer {
|
||||
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
|
||||
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
|
||||
() -> Math.pow(driver.getLeftY(), 3),
|
||||
() -> Math.pow(driver.getLeftX(), 3),
|
||||
() -> driver.getRightX(),
|
||||
() -> true
|
||||
)
|
||||
);
|
||||
@@ -167,6 +167,15 @@ public class RobotContainer {
|
||||
|
||||
driver.start().whileTrue(drivetrain.resetToVision());
|
||||
|
||||
driver.povUp().whileTrue(
|
||||
drivetrain.resetToVision().andThen(
|
||||
drivetrain.goToPose(
|
||||
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0],
|
||||
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1],
|
||||
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
|
||||
))
|
||||
);
|
||||
|
||||
driver.rightBumper().whileTrue(
|
||||
drivetrain.resetToVision().andThen(
|
||||
drivetrain.goToPose(
|
||||
@@ -217,8 +226,8 @@ public class RobotContainer {
|
||||
operator.back().onTrue(elevator.homeCommand());
|
||||
|
||||
operator.start().toggleOnTrue(
|
||||
climberPivot.runPivot(() -> -operator.getRightY()*0.5)
|
||||
.alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
|
||||
climberPivot.runPivot(() -> -operator.getRightY())
|
||||
.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
|
||||
|
||||
operator.a().onTrue(
|
||||
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
|
||||
@@ -330,7 +339,7 @@ public class RobotContainer {
|
||||
),
|
||||
manipulatorPivot.maintainPosition()
|
||||
.withTimeout(
|
||||
0.01
|
||||
0.1
|
||||
)
|
||||
)
|
||||
);
|
||||
@@ -342,6 +351,28 @@ public class RobotContainer {
|
||||
ManipulatorPivotConstants.kStartingPosition
|
||||
)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Shoot Algae",
|
||||
shootAlgae().withTimeout(2)
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Processor Position",
|
||||
moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
|
||||
.raceWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
);
|
||||
|
||||
NamedCommands.registerCommand(
|
||||
"Pickup Algae L2",
|
||||
moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
|
||||
.raceWith(manipulator.runManipulator(() -> 0.85, false))
|
||||
.andThen(
|
||||
elevator.maintainPosition()
|
||||
.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
|
||||
|
||||
//Dont you need a holdPosition call?
|
||||
);
|
||||
}
|
||||
|
||||
//creates tabs and transforms them on the shuffleboard
|
||||
@@ -393,26 +424,6 @@ public class RobotContainer {
|
||||
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
|
||||
.withWidget(BuiltInWidgets.kGraph);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 0)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
|
||||
.withSize(1, 1)
|
||||
.withPosition(5, 1)
|
||||
.withWidget(BuiltInWidgets.kTextView);
|
||||
|
||||
sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
|
||||
|
||||
sensorTab.addDouble("velocity", drivetrain::getVelocity);
|
||||
@@ -598,10 +609,10 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
private Command shootAlgae(){
|
||||
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
|
||||
elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
|
||||
elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
|
||||
.raceWith(elevator.maintainPosition()));
|
||||
}
|
||||
|
||||
|
||||
@@ -16,6 +16,8 @@ public class AutoConstants {
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
|
||||
|
||||
public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
|
||||
|
||||
public static final double kPXYController = 3.5;
|
||||
public static final double kPThetaController = 5;
|
||||
|
||||
|
||||
@@ -1,9 +1,18 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
public class ClimberRollersConstants {
|
||||
public static final int kRollerMotorID = 9;
|
||||
|
||||
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
motorConfig
|
||||
.smartCurrentLimit(40)
|
||||
.idleMode(IdleMode.kBrake)
|
||||
.inverted(true);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
public class DrivetrainConstants {
|
||||
// Driving Parameters - Note that these are not the maximum capable speeds of
|
||||
// the robot, rather the allowed maximum speeds
|
||||
public static final double kMaxSpeedMetersPerSecond = 5.5;
|
||||
public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
|
||||
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
|
||||
|
||||
// Chassis configuration
|
||||
|
||||
@@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Second;
|
||||
import static edu.wpi.first.units.Units.Seconds;
|
||||
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
@@ -22,6 +23,7 @@ public class ElevatorConstants {
|
||||
|
||||
public static final int kCurrentLimit = 40;
|
||||
|
||||
/*
|
||||
public static final double kUpControllerP = 5.6;//7; //
|
||||
public static final double kUpControllerI = 0;
|
||||
public static final double kUpControllerD = 0.28;//0.28
|
||||
@@ -31,15 +33,22 @@ public class ElevatorConstants {
|
||||
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
|
||||
|
||||
public static final double kMaintainP = 3;
|
||||
|
||||
*/
|
||||
|
||||
public static final double kP = 3;//7; //
|
||||
public static final double kI = 0;
|
||||
public static final double kD = 0;//.28;//0.28
|
||||
|
||||
public static final double kAllowedError = 1;
|
||||
|
||||
public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */
|
||||
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
|
||||
public static final double kFeedForwardV = 0.12; // calculated value 0.12
|
||||
|
||||
public static final double kMaxVelocity = 150.0; // 120 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
public static final double kMaxVelocity = 100.0; // 100 inches per second (COOKING) calculated max is 184 in/s
|
||||
public static final double kMaxAcceleration = 50; // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2
|
||||
public static final double kMaxVelocityAlgae = 120;
|
||||
public static final double kMaxAccelerationAlgae = 400;
|
||||
|
||||
public static final double kCoralIntakePosition = 0;
|
||||
public static final double kL1Position = 17;
|
||||
@@ -56,6 +65,8 @@ public class ElevatorConstants {
|
||||
|
||||
public static final double kVoltageLimit = 7;
|
||||
|
||||
public static final double kVoltageLimitAlgae = 9;
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = .25;
|
||||
public static final double kSysIDStepVolts = 3;
|
||||
@@ -81,5 +92,9 @@ public class ElevatorConstants {
|
||||
motorConfig.encoder
|
||||
.positionConversionFactor(kEncoderPositionConversionFactor)
|
||||
.velocityConversionFactor(kEncoderVelocityConversionFactor);
|
||||
motorConfig.closedLoop
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.pid(kP, kI, kD)
|
||||
.outputRange(-1, 1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -23,7 +23,9 @@ public class ManipulatorPivotConstants {
|
||||
public static final double kPositionalP = 4.5;
|
||||
public static final double kPositionalI = 0;
|
||||
public static final double kPositionalD = 0;
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
|
||||
public static final double kPositionalTolerance = Units.degreesToRadians(3);
|
||||
|
||||
public static final double kAlgaeP = 7;
|
||||
|
||||
public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
|
||||
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
|
||||
|
||||
@@ -59,6 +59,32 @@ public class VisionConstants {
|
||||
{4.993, 2.816, 5.272, 2.996}
|
||||
};
|
||||
|
||||
public static final double[][] algaeSetpointsMap = {
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{13.906, 2.658},//6
|
||||
{14.661, 4.013},
|
||||
{13.834, 5.428},
|
||||
{12.263, 5.452},
|
||||
{11.412, 4.025},
|
||||
{12.191, 2.574},//11
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
{3.649, 2.558},//17
|
||||
{2.776, 4.005},
|
||||
{3.644, 5.514},
|
||||
{5.296, 5.522},//4.991, 5.246},
|
||||
{6.225, 4.008},
|
||||
{5.322, 2.511}//22
|
||||
};
|
||||
|
||||
public static final double latencyFudge = 0.0;
|
||||
|
||||
}
|
||||
|
||||
@@ -177,11 +177,17 @@ public class Drivetrain extends SubsystemBase {
|
||||
|
||||
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
|
||||
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
|
||||
|
||||
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
|
||||
if(vision.getOrangeDist() < 60){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
|
||||
if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
|
||||
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
|
||||
}else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
|
||||
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
|
||||
}
|
||||
|
||||
// if the detected tags match your alliances reef tags use their pose estimates
|
||||
@@ -195,11 +201,21 @@ public class Drivetrain extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
|
||||
Logger.recordOutput("orange dist", vision.getOrangeDist());
|
||||
Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
|
||||
Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
|
||||
Logger.recordOutput("orange FPS", vision.getOrangeFPS());
|
||||
|
||||
|
||||
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
|
||||
if(vision.getBlackDist() < 60){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
|
||||
if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
|
||||
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
|
||||
}else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
|
||||
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
|
||||
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
|
||||
}
|
||||
|
||||
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
|
||||
@@ -212,7 +228,13 @@ public class Drivetrain extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
|
||||
|
||||
Logger.recordOutput("black dist", vision.getBlackDist());
|
||||
Logger.recordOutput("black detected", vision.getBlackTagDetected());
|
||||
Logger.recordOutput("black tag", vision.getBlackTagDetected());
|
||||
Logger.recordOutput("black FPS", vision.getBlackFPS());
|
||||
|
||||
Logger.recordOutput("drive velocity", getVelocity());
|
||||
Logger.recordOutput("closest tag", getClosestTag());
|
||||
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
|
||||
|
||||
if(musicTimer.get()>10){
|
||||
@@ -340,8 +362,8 @@ public class Drivetrain extends SubsystemBase {
|
||||
);
|
||||
|
||||
double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
|
||||
if (speed > AutoConstants.kMaxSpeedMetersPerSecond) {
|
||||
double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed;
|
||||
if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
|
||||
double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
|
||||
controlEffort.vxMetersPerSecond *= mul;
|
||||
controlEffort.vyMetersPerSecond *= mul;
|
||||
}
|
||||
|
||||
@@ -2,15 +2,20 @@ package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.ClosedLoopSlot;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.ControlType;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.ElevatorFeedforward;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@@ -24,12 +29,14 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
private DigitalInput bottomLimitSwitch;
|
||||
|
||||
private PIDController pidControllerUp;
|
||||
private PIDController pidControllerDown;
|
||||
|
||||
private PIDController maintainPID;
|
||||
|
||||
private ElevatorFeedforward feedForward;
|
||||
|
||||
private TrapezoidProfile trapProfile;
|
||||
private TrapezoidProfile trapProfileAlgae;
|
||||
private TrapezoidProfile.State goal;
|
||||
private TrapezoidProfile.State setpoint;
|
||||
|
||||
private SparkClosedLoopController controller;
|
||||
|
||||
public Elevator() {
|
||||
elevatorMotor1 = new SparkMax(
|
||||
@@ -60,33 +67,17 @@ public class Elevator extends SubsystemBase {
|
||||
ElevatorConstants.kBottomLimitSwitchID
|
||||
);
|
||||
|
||||
pidControllerDown = new PIDController(
|
||||
ElevatorConstants.kDownControllerP,
|
||||
ElevatorConstants.kDownControllerI,
|
||||
ElevatorConstants.kDownControllerD
|
||||
);
|
||||
pidControllerDown.setSetpoint(0);
|
||||
|
||||
pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
pidControllerUp = new PIDController(
|
||||
ElevatorConstants.kUpControllerP,
|
||||
ElevatorConstants.kUpControllerI,
|
||||
ElevatorConstants.kUpControllerD
|
||||
);
|
||||
pidControllerUp.setSetpoint(0);
|
||||
|
||||
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
|
||||
|
||||
maintainPID.setTolerance(ElevatorConstants.kAllowedError);
|
||||
|
||||
feedForward = new ElevatorFeedforward(
|
||||
ElevatorConstants.kFeedForwardS,
|
||||
ElevatorConstants.kFeedForwardG,
|
||||
ElevatorConstants.kFeedForwardV
|
||||
);
|
||||
|
||||
trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
|
||||
|
||||
trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
|
||||
|
||||
controller = elevatorMotor1.getClosedLoopController();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -94,6 +85,8 @@ public class Elevator extends SubsystemBase {
|
||||
if (!getBottomLimitSwitch()) {
|
||||
encoder.setPosition(0);
|
||||
}
|
||||
|
||||
Logger.recordOutput("elevator position", getEncoderPosition());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -147,28 +140,15 @@ public class Elevator extends SubsystemBase {
|
||||
public Command maintainPosition() {
|
||||
|
||||
return startRun(() -> {
|
||||
maintainPID.reset();
|
||||
maintainPID.setSetpoint(pidControllerUp.getSetpoint());
|
||||
|
||||
},
|
||||
() -> {
|
||||
|
||||
double maintainOutput = maintainPID.calculate(getEncoderPosition());
|
||||
|
||||
if(!maintainPID.atSetpoint())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
maintainOutput + feedForward.calculate(0), -2, 2)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
*/
|
||||
controller.setReference(
|
||||
encoder.getPosition(),
|
||||
ControlType.kPosition,
|
||||
ClosedLoopSlot.kSlot0,
|
||||
feedForward.calculate(0.0)
|
||||
);
|
||||
|
||||
});
|
||||
|
||||
@@ -186,109 +166,42 @@ public class Elevator extends SubsystemBase {
|
||||
* Moves the elevator to a target destination (setpoint).
|
||||
*
|
||||
* @param setpoint Target destination of the subsystem
|
||||
* @param timeout Time to achieve the setpoint before quitting
|
||||
* @return Sets motor voltage to achieve the target destination
|
||||
*/
|
||||
public Command goToSetpoint(DoubleSupplier setpoint) {
|
||||
|
||||
if (setpoint.getAsDouble() == 0) {
|
||||
return startRun(() -> {
|
||||
public Command goToSetpoint(DoubleSupplier setGoal) {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
|
||||
.andThen(runManualElevator(() -> -.5)
|
||||
.until(() -> encoder.getPosition() == 0));
|
||||
return startRun(() -> {
|
||||
goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
|
||||
}, () -> {
|
||||
setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
|
||||
|
||||
} else {
|
||||
return startRun(() -> {
|
||||
|
||||
pidControllerUp.reset();
|
||||
pidControllerDown.reset();
|
||||
pidControllerUp.setSetpoint(setpoint.getAsDouble());
|
||||
pidControllerDown.setSetpoint(setpoint.getAsDouble());
|
||||
|
||||
},
|
||||
() -> {
|
||||
double upOutput = pidControllerUp.calculate(getEncoderPosition());
|
||||
double downOutput = pidControllerDown.calculate(getEncoderPosition());
|
||||
|
||||
if(setpoint.getAsDouble()>encoder.getPosition())
|
||||
elevatorMotor1.setVoltage( MathUtil.clamp(
|
||||
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
else{
|
||||
elevatorMotor1.setVoltage(
|
||||
MathUtil.clamp(
|
||||
downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
|
||||
);
|
||||
}
|
||||
|
||||
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
controller.setReference(
|
||||
setpoint.position,
|
||||
ControlType.kPosition,
|
||||
ClosedLoopSlot.kSlot0,
|
||||
feedForward.calculate(encoder.getVelocity())
|
||||
);
|
||||
*/
|
||||
/*
|
||||
if (!pidController.atSetpoint()) {
|
||||
elevatorMotor1.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(0)
|
||||
);
|
||||
} else {
|
||||
elevatorMotor1.setVoltage(
|
||||
feedForward.calculate(0)
|
||||
);
|
||||
}
|
||||
|
||||
});*/
|
||||
}
|
||||
|
||||
}).until(() -> trapProfile.isFinished(encoder.getPosition()));
|
||||
|
||||
}
|
||||
|
||||
public Command goToSetpointAlgae(DoubleSupplier setGoal) {
|
||||
|
||||
/*
|
||||
if(encoder.getPosition() >= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerUp.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
||||
);
|
||||
}else if(encoder.getPosition() <= setpoint.getAsDouble()){
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerDown.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
|
||||
);
|
||||
}
|
||||
*/
|
||||
return startRun(() -> {
|
||||
goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
|
||||
}, () -> {
|
||||
setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
|
||||
|
||||
controller.setReference(
|
||||
setpoint.position,
|
||||
ControlType.kPosition,
|
||||
ClosedLoopSlot.kSlot0,
|
||||
feedForward.calculate(setpoint.velocity)
|
||||
);
|
||||
|
||||
}).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current encoder position
|
||||
@@ -327,19 +240,4 @@ public class Elevator extends SubsystemBase {
|
||||
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
|
||||
}
|
||||
|
||||
public double getPIDUpSetpoint() {
|
||||
return pidControllerUp.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDUpError() {
|
||||
return pidControllerUp.getError();
|
||||
}
|
||||
|
||||
public double getPIDDownSetpoint() {
|
||||
return pidControllerDown.getSetpoint();
|
||||
}
|
||||
|
||||
public double getPIDDownError() {
|
||||
return pidControllerDown.getError();
|
||||
}
|
||||
}
|
||||
@@ -2,6 +2,8 @@ package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
@@ -46,6 +48,13 @@ public class Manipulator extends SubsystemBase {
|
||||
coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
super.periodic();
|
||||
|
||||
Logger.recordOutput("coral beam break", getCoralBeamBreak());
|
||||
}
|
||||
|
||||
/**
|
||||
* The default command for the manipulator that either stops the manipulator or slowly
|
||||
* runs the manipulator to retain the algae
|
||||
@@ -84,8 +93,8 @@ public class Manipulator extends SubsystemBase {
|
||||
*/
|
||||
public Command runUntilCollected(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
manipulatorMotor.set(
|
||||
speed.getAsDouble()
|
||||
manipulatorMotor.setVoltage(
|
||||
speed.getAsDouble()*12
|
||||
);
|
||||
|
||||
indexerMotor.set(1);
|
||||
|
||||
@@ -4,6 +4,8 @@ import com.revrobotics.spark.SparkMax;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.spark.SparkAbsoluteEncoder;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
@@ -25,6 +27,8 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
|
||||
private PIDController pidController;
|
||||
|
||||
private PIDController algaePIDController;
|
||||
|
||||
public ManipulatorPivot() {
|
||||
pivotMotor = new SparkMax(
|
||||
ManipulatorPivotConstants.kPivotMotorID,
|
||||
@@ -44,10 +48,22 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
ManipulatorPivotConstants.kPositionalI,
|
||||
ManipulatorPivotConstants.kPositionalD
|
||||
);
|
||||
pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
|
||||
|
||||
pidController.setSetpoint(0);
|
||||
|
||||
pidController.enableContinuousInput(0, 280);
|
||||
|
||||
algaePIDController = new PIDController(
|
||||
ManipulatorPivotConstants.kAlgaeP,
|
||||
0,
|
||||
0);
|
||||
algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
|
||||
|
||||
algaePIDController.setSetpoint(0);
|
||||
|
||||
algaePIDController.enableContinuousInput(0, 280);
|
||||
|
||||
feedForward = new ArmFeedforward(
|
||||
ManipulatorPivotConstants.kFeedForwardS,
|
||||
ManipulatorPivotConstants.kFeedForwardG,
|
||||
@@ -55,6 +71,14 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
super.periodic();
|
||||
|
||||
Logger.recordOutput("manipulator position", getEncoderPosition());
|
||||
Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether or not the motion is safe relative to the encoder's current position
|
||||
* and the arm safe stow position
|
||||
@@ -125,6 +149,38 @@ public class ManipulatorPivot extends SubsystemBase {
|
||||
}).until(() -> pidController.atSetpoint());
|
||||
}
|
||||
|
||||
public Command goToSetpointAlgae(DoubleSupplier setpoint) {
|
||||
return startRun(() -> {
|
||||
|
||||
algaePIDController.setSetpoint(setpoint.getAsDouble());
|
||||
algaePIDController.reset();
|
||||
pidController.setSetpoint(setpoint.getAsDouble());
|
||||
pidController.reset();
|
||||
},
|
||||
() -> {
|
||||
/*
|
||||
if (!pidController.atSetpoint()) {
|
||||
pivotMotor.setVoltage(
|
||||
pidController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint.getAsDouble()
|
||||
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
|
||||
);
|
||||
} else {
|
||||
pivotMotor.setVoltage(
|
||||
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
|
||||
);
|
||||
}
|
||||
*/
|
||||
pivotMotor.setVoltage(
|
||||
algaePIDController.calculate(
|
||||
encoder.getPosition(),
|
||||
setpoint.getAsDouble()
|
||||
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
|
||||
);
|
||||
}).until(() -> algaePIDController.atSetpoint());
|
||||
}
|
||||
|
||||
public Command maintainPosition() {
|
||||
return startRun(() -> {
|
||||
|
||||
|
||||
@@ -177,11 +177,11 @@ public class Vision{
|
||||
}
|
||||
|
||||
public boolean isBlackConnected(){
|
||||
return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0;
|
||||
return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
|
||||
}
|
||||
|
||||
public boolean isOrangeConnected(){
|
||||
return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0;
|
||||
return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user