mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Merge branch 'master' of https://github.com/Team4388/2026KPopRobotHunters
This commit is contained in:
mimigamin
+86
-55
@@ -8,10 +8,10 @@
|
|||||||
"layouts": [
|
"layouts": [
|
||||||
{
|
{
|
||||||
"title": "Tag Processed",
|
"title": "Tag Processed",
|
||||||
"x": 384.0,
|
"x": 896.0,
|
||||||
"y": 0.0,
|
"y": 0.0,
|
||||||
"width": 256.0,
|
"width": 256.0,
|
||||||
"height": 256.0,
|
"height": 384.0,
|
||||||
"type": "List Layout",
|
"type": "List Layout",
|
||||||
"properties": {
|
"properties": {
|
||||||
"label_position": "TOP"
|
"label_position": "TOP"
|
||||||
@@ -55,27 +55,11 @@
|
|||||||
}
|
}
|
||||||
],
|
],
|
||||||
"containers": [
|
"containers": [
|
||||||
{
|
|
||||||
"title": "MatchTime",
|
|
||||||
"x": 0.0,
|
|
||||||
"y": 0.0,
|
|
||||||
"width": 384.0,
|
|
||||||
"height": 256.0,
|
|
||||||
"type": "Match Time",
|
|
||||||
"properties": {
|
|
||||||
"topic": "/AdvantageKit/DriverStation/MatchTime",
|
|
||||||
"period": 0.06,
|
|
||||||
"data_type": "double",
|
|
||||||
"time_display_mode": "Minutes and Seconds",
|
|
||||||
"red_start_time": 15,
|
|
||||||
"yellow_start_time": 30
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"title": "RetractedLimit",
|
"title": "RetractedLimit",
|
||||||
"x": 0.0,
|
"x": 512.0,
|
||||||
"y": 384.0,
|
"y": 0.0,
|
||||||
"width": 128.0,
|
"width": 256.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Boolean Box",
|
"type": "Boolean Box",
|
||||||
"properties": {
|
"properties": {
|
||||||
@@ -90,9 +74,9 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Auto Chooser",
|
"title": "Auto Chooser",
|
||||||
"x": 1024.0,
|
"x": 896.0,
|
||||||
"y": 256.0,
|
"y": 384.0,
|
||||||
"width": 384.0,
|
"width": 256.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "ComboBox Chooser",
|
"type": "ComboBox Chooser",
|
||||||
"properties": {
|
"properties": {
|
||||||
@@ -103,9 +87,9 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Roller Percent Output",
|
"title": "Roller Percent Output",
|
||||||
"x": 0.0,
|
"x": 512.0,
|
||||||
"y": 256.0,
|
"y": 256.0,
|
||||||
"width": 256.0,
|
"width": 384.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Number Slider",
|
"type": "Number Slider",
|
||||||
"properties": {
|
"properties": {
|
||||||
@@ -120,8 +104,8 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Shooter OVERRIDE Velocity",
|
"title": "Shooter OVERRIDE Velocity",
|
||||||
"x": 128.0,
|
"x": 768.0,
|
||||||
"y": 384.0,
|
"y": 0.0,
|
||||||
"width": 128.0,
|
"width": 128.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Text Display",
|
"type": "Text Display",
|
||||||
@@ -134,8 +118,8 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "TRIM SHOOTER SPEED",
|
"title": "TRIM SHOOTER SPEED",
|
||||||
"x": 256.0,
|
"x": 512.0,
|
||||||
"y": 256.0,
|
"y": 384.0,
|
||||||
"width": 384.0,
|
"width": 384.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Number Slider",
|
"type": "Number Slider",
|
||||||
@@ -151,8 +135,8 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Mode",
|
"title": "Mode",
|
||||||
"x": 256.0,
|
"x": 512.0,
|
||||||
"y": 384.0,
|
"y": 128.0,
|
||||||
"width": 384.0,
|
"width": 384.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Text Display",
|
"type": "Text Display",
|
||||||
@@ -164,30 +148,49 @@
|
|||||||
}
|
}
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Time to rev",
|
"title": "IsActive",
|
||||||
"x": 768.0,
|
"x": 0.0,
|
||||||
"y": 128.0,
|
"y": 0.0,
|
||||||
"width": 256.0,
|
"width": 384.0,
|
||||||
"height": 128.0,
|
"height": 256.0,
|
||||||
"type": "Large Text Display",
|
"type": "Boolean Box",
|
||||||
"properties": {
|
"properties": {
|
||||||
"topic": "/AdvantageKit/RealOutputs/Time to rev",
|
"topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
|
||||||
"period": 0.06,
|
"period": 0.06,
|
||||||
"data_type": "double"
|
"data_type": "boolean",
|
||||||
|
"true_color": 4283215696,
|
||||||
|
"false_color": 4294198070,
|
||||||
|
"true_icon": "None",
|
||||||
|
"false_icon": "None"
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Arm angle extended",
|
"title": "RemainingInShift",
|
||||||
"x": 1152.0,
|
"x": 0.0,
|
||||||
"y": 128.0,
|
"y": 256.0,
|
||||||
"width": 256.0,
|
"width": 384.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Text Display",
|
"type": "Match Time",
|
||||||
"properties": {
|
"properties": {
|
||||||
"topic": "/SmartDashboard/Arm angle extended",
|
"topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
|
||||||
"period": 0.06,
|
"period": 0.06,
|
||||||
"data_type": "double",
|
"data_type": "double",
|
||||||
"show_submit_button": true
|
"time_display_mode": "Minutes and Seconds",
|
||||||
|
"red_start_time": 15,
|
||||||
|
"yellow_start_time": 30
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"title": "Phase",
|
||||||
|
"x": 0.0,
|
||||||
|
"y": 384.0,
|
||||||
|
"width": 384.0,
|
||||||
|
"height": 128.0,
|
||||||
|
"type": "Large Text Display",
|
||||||
|
"properties": {
|
||||||
|
"topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
|
||||||
|
"period": 0.06,
|
||||||
|
"data_type": "string"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
@@ -527,8 +530,8 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Shooter Idle max current",
|
"title": "Shooter Idle max current",
|
||||||
"x": 384.0,
|
"x": 1024.0,
|
||||||
"y": 512.0,
|
"y": 384.0,
|
||||||
"width": 256.0,
|
"width": 256.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Text Display",
|
"type": "Text Display",
|
||||||
@@ -555,8 +558,8 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"title": "Shooter OVERRIDE Velocity",
|
"title": "Shooter OVERRIDE Velocity",
|
||||||
"x": 640.0,
|
"x": 1024.0,
|
||||||
"y": 512.0,
|
"y": 256.0,
|
||||||
"width": 256.0,
|
"width": 256.0,
|
||||||
"height": 128.0,
|
"height": 128.0,
|
||||||
"type": "Text Display",
|
"type": "Text Display",
|
||||||
@@ -575,12 +578,40 @@
|
|||||||
"grid_layout": {
|
"grid_layout": {
|
||||||
"layouts": [],
|
"layouts": [],
|
||||||
"containers": [
|
"containers": [
|
||||||
|
{
|
||||||
|
"title": "Stalled Motor: ",
|
||||||
|
"x": 512.0,
|
||||||
|
"y": 128.0,
|
||||||
|
"width": 384.0,
|
||||||
|
"height": 128.0,
|
||||||
|
"type": "Text Display",
|
||||||
|
"properties": {
|
||||||
|
"topic": "/AdvantageKit/RealOutputs/Stalled Motor: ",
|
||||||
|
"period": 0.06,
|
||||||
|
"data_type": "string",
|
||||||
|
"show_submit_button": false
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"title": "Shooter idle % output",
|
||||||
|
"x": 128.0,
|
||||||
|
"y": 256.0,
|
||||||
|
"width": 256.0,
|
||||||
|
"height": 128.0,
|
||||||
|
"type": "Text Display",
|
||||||
|
"properties": {
|
||||||
|
"topic": "/SmartDashboard/Shooter idle % output",
|
||||||
|
"period": 0.06,
|
||||||
|
"data_type": "double",
|
||||||
|
"show_submit_button": true
|
||||||
|
}
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"title": "Auto Chooser",
|
"title": "Auto Chooser",
|
||||||
"x": 0.0,
|
"x": 512.0,
|
||||||
"y": 0.0,
|
"y": 256.0,
|
||||||
"width": 640.0,
|
"width": 384.0,
|
||||||
"height": 768.0,
|
"height": 128.0,
|
||||||
"type": "ComboBox Chooser",
|
"type": "ComboBox Chooser",
|
||||||
"properties": {
|
"properties": {
|
||||||
"topic": "/SmartDashboard/Auto Chooser",
|
"topic": "/SmartDashboard/Auto Chooser",
|
||||||
|
|||||||
@@ -0,0 +1,44 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Robot Rev Up"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "parallel",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Robot Shoot"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "Shoot driving across"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Robot Shoot Driving"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
@@ -0,0 +1,43 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"command": {
|
||||||
|
"type": "sequential",
|
||||||
|
"data": {
|
||||||
|
"commands": [
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "Intake Extended"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "Bump test"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "named",
|
||||||
|
"data": {
|
||||||
|
"name": "BumpOffsetForward"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "BumpToCenter"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "path",
|
||||||
|
"data": {
|
||||||
|
"pathName": "HubFarLeft-NeutralZone 2-2"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"resetOdom": true,
|
||||||
|
"folder": null,
|
||||||
|
"choreoAuto": false
|
||||||
|
}
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.4908076923076936,
|
||||||
|
"y": 6.633345195729537
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 2.3977564102564113,
|
||||||
|
"y": 6.0868195547038955
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 3.4908076923076936,
|
||||||
|
"y": 5.6745769230769225
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 2.6330609248104158,
|
||||||
|
"y": 5.743196664476699
|
||||||
|
},
|
||||||
|
"nextControl": {
|
||||||
|
"x": 3.7815128205128214,
|
||||||
|
"y": 5.651320512820514
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.967615384615385,
|
||||||
|
"y": 5.709461538461539
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 4.746653846153847,
|
||||||
|
"y": 5.430384615384616
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [
|
||||||
|
{
|
||||||
|
"waypointRelativePos": 1.5211640211640187,
|
||||||
|
"rotationDegrees": -112.5
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 600.0,
|
||||||
|
"maxAngularAcceleration": 750.0,
|
||||||
|
"nominalVoltage": 10.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -91.24536426676825
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 179.59775645089275
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
@@ -0,0 +1,54 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 5.935255041518388,
|
||||||
|
"y": 5.6650177935943065
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 6.073930090574693,
|
||||||
|
"y": 5.873030367178768
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 7.559893238434164,
|
||||||
|
"y": 7.547876631079477
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 7.1940806642941855,
|
||||||
|
"y": 7.182064056939502
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 3.0,
|
||||||
|
"maxAcceleration": 3.0,
|
||||||
|
"maxAngularVelocity": 600.0,
|
||||||
|
"maxAngularAcceleration": 750.0,
|
||||||
|
"nominalVoltage": 10.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -90.98776039963968
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": -91.19348942398209
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": true
|
||||||
|
}
|
||||||
@@ -58,7 +58,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -104,7 +104,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -48,7 +48,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -88,7 +88,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -88,7 +88,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -64,7 +64,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -58,7 +58,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -48,7 +48,7 @@
|
|||||||
"maxAcceleration": 3.0,
|
"maxAcceleration": 3.0,
|
||||||
"maxAngularVelocity": 600.0,
|
"maxAngularVelocity": 600.0,
|
||||||
"maxAngularAcceleration": 750.0,
|
"maxAngularAcceleration": 750.0,
|
||||||
"nominalVoltage": 12.0,
|
"nominalVoltage": 10.0,
|
||||||
"unlimited": false
|
"unlimited": false
|
||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
|
|||||||
@@ -0,0 +1,102 @@
|
|||||||
|
{
|
||||||
|
"version": "2025.0",
|
||||||
|
"waypoints": [
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.9326282051282067,
|
||||||
|
"y": 6.5815769230769225
|
||||||
|
},
|
||||||
|
"prevControl": null,
|
||||||
|
"nextControl": {
|
||||||
|
"x": 1.9932621113872893,
|
||||||
|
"y": 6.3390412980405895
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.9326282051282067,
|
||||||
|
"y": 1.6628461538461545
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 1.936134248900721,
|
||||||
|
"y": 1.9128215679513527
|
||||||
|
},
|
||||||
|
"nextControl": {
|
||||||
|
"x": 1.9291221613556924,
|
||||||
|
"y": 1.4128707397409563
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.9326282051282067,
|
||||||
|
"y": 6.5815769230769225
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 1.9198835421653975,
|
||||||
|
"y": 6.331901987278464
|
||||||
|
},
|
||||||
|
"nextControl": {
|
||||||
|
"x": 1.9453728680910158,
|
||||||
|
"y": 6.8312518588753814
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.9326282051282067,
|
||||||
|
"y": 1.6628461538461545
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 1.9337461053582308,
|
||||||
|
"y": 1.912843654431812
|
||||||
|
},
|
||||||
|
"nextControl": {
|
||||||
|
"x": 1.9315103048981825,
|
||||||
|
"y": 1.4128486532604971
|
||||||
|
},
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"anchor": {
|
||||||
|
"x": 1.9326282051282067,
|
||||||
|
"y": 6.5815769230769225
|
||||||
|
},
|
||||||
|
"prevControl": {
|
||||||
|
"x": 1.8878500514131944,
|
||||||
|
"y": 6.335619778537727
|
||||||
|
},
|
||||||
|
"nextControl": null,
|
||||||
|
"isLocked": false,
|
||||||
|
"linkedName": null
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"rotationTargets": [],
|
||||||
|
"constraintZones": [],
|
||||||
|
"pointTowardsZones": [],
|
||||||
|
"eventMarkers": [],
|
||||||
|
"globalConstraints": {
|
||||||
|
"maxVelocity": 0.8,
|
||||||
|
"maxAcceleration": 10.0,
|
||||||
|
"maxAngularVelocity": 600.0,
|
||||||
|
"maxAngularAcceleration": 750.0,
|
||||||
|
"nominalVoltage": 12.0,
|
||||||
|
"unlimited": false
|
||||||
|
},
|
||||||
|
"goalEndState": {
|
||||||
|
"velocity": 0,
|
||||||
|
"rotation": 180.0
|
||||||
|
},
|
||||||
|
"reversed": false,
|
||||||
|
"folder": null,
|
||||||
|
"idealStartingState": {
|
||||||
|
"velocity": 0.0,
|
||||||
|
"rotation": 180.0
|
||||||
|
},
|
||||||
|
"useDefaultConstraints": false
|
||||||
|
}
|
||||||
@@ -15,11 +15,14 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
|
|||||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||||
|
|
||||||
|
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||||
import frc4388.robot.constants.BuildConstants;
|
import frc4388.robot.constants.BuildConstants;
|
||||||
import frc4388.robot.constants.Constants.SimConstants;
|
import frc4388.robot.constants.Constants.SimConstants;
|
||||||
import frc4388.utility.DeferredBlock;
|
import frc4388.utility.DeferredBlock;
|
||||||
|
import frc4388.utility.compute.HubShiftTimer;
|
||||||
|
import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
|
||||||
import frc4388.utility.compute.RobotTime;
|
import frc4388.utility.compute.RobotTime;
|
||||||
import frc4388.utility.compute.Trim;
|
import frc4388.utility.compute.Trim;
|
||||||
import frc4388.utility.status.FaultReporter;
|
import frc4388.utility.status.FaultReporter;
|
||||||
@@ -113,6 +116,7 @@ public class Robot extends LoggedRobot {
|
|||||||
m_autonomousCommand.schedule();
|
m_autonomousCommand.schedule();
|
||||||
}
|
}
|
||||||
m_robotTime.startMatchTime();
|
m_robotTime.startMatchTime();
|
||||||
|
HubShiftTimer.initializeAuto();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -138,6 +142,7 @@ public class Robot extends LoggedRobot {
|
|||||||
|
|
||||||
}
|
}
|
||||||
m_robotTime.startMatchTime();
|
m_robotTime.startMatchTime();
|
||||||
|
HubShiftTimer.initializeTeleop();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -145,7 +150,12 @@ public class Robot extends LoggedRobot {
|
|||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
public void teleopPeriodic() {
|
public void teleopPeriodic() {
|
||||||
// m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
|
var info = HubShiftTimer.getShiftInfo();
|
||||||
|
|
||||||
|
double rumble = (info.remainingInShift() < 5. && info.remainingInShift() > 0.1) ? 1 : 0;
|
||||||
|
|
||||||
|
// m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble);
|
||||||
|
// m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -5,9 +5,14 @@
|
|||||||
/* the project. */
|
/* the 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 frc4388.robot;
|
package frc4388.robot;
|
||||||
|
|
||||||
import java.io.File;
|
import java.io.File;
|
||||||
|
import java.util.Optional;
|
||||||
|
|
||||||
import com.pathplanner.lib.auto.NamedCommands;
|
import com.pathplanner.lib.auto.NamedCommands;
|
||||||
import com.pathplanner.lib.commands.PathPlannerAuto;
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||||
@@ -24,24 +29,27 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|||||||
// Commands
|
// Commands
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||||
|
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||||
|
import frc4388.robot.commands.Swerve.StayInPosition;
|
||||||
import frc4388.robot.commands.alignment.AutoAlign;
|
import frc4388.robot.commands.alignment.AutoAlign;
|
||||||
import frc4388.robot.constants.Constants;
|
import frc4388.robot.constants.Constants;
|
||||||
|
import frc4388.robot.constants.FieldConstants;
|
||||||
import frc4388.robot.constants.Constants.OIConstants;
|
import frc4388.robot.constants.Constants.OIConstants;
|
||||||
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||||
// Subsystems
|
|
||||||
import frc4388.robot.subsystems.LED;
|
|
||||||
import frc4388.robot.subsystems.Lidar;
|
|
||||||
import frc4388.robot.subsystems.intake.Intake;
|
import frc4388.robot.subsystems.intake.Intake;
|
||||||
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
import frc4388.robot.subsystems.intake.Intake.IntakeMode;
|
||||||
|
import frc4388.robot.subsystems.led.LED;
|
||||||
import frc4388.robot.subsystems.shooter.Shooter;
|
import frc4388.robot.subsystems.shooter.Shooter;
|
||||||
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
||||||
|
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||||
|
import frc4388.robot.subsystems.vision.Lidar;
|
||||||
import frc4388.robot.subsystems.vision.Vision;
|
import frc4388.robot.subsystems.vision.Vision;
|
||||||
import frc4388.utility.DeferredBlock;
|
import frc4388.utility.DeferredBlock;
|
||||||
import frc4388.utility.compute.FieldPositions;
|
import frc4388.utility.compute.FieldPositions;
|
||||||
@@ -61,7 +69,7 @@ import frc4388.utility.controller.XboxController;
|
|||||||
public class RobotContainer {
|
public class RobotContainer {
|
||||||
/* RobotMap */
|
/* RobotMap */
|
||||||
|
|
||||||
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
|
public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM);
|
||||||
|
|
||||||
/*Limit Switch */
|
/*Limit Switch */
|
||||||
// public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
|
// public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
|
||||||
@@ -69,6 +77,7 @@ public class RobotContainer {
|
|||||||
/* Subsystems */
|
/* Subsystems */
|
||||||
// public final Lidar m_lidar = new Lidar();
|
// public final Lidar m_lidar = new Lidar();
|
||||||
public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
|
public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
|
||||||
|
public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim();
|
||||||
//Testing of Colors
|
//Testing of Colors
|
||||||
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
|
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
|
||||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||||
@@ -84,156 +93,212 @@ public class RobotContainer {
|
|||||||
// private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
// private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
||||||
|
|
||||||
// public List<Subsystem> subsystems = new ArrayList<>();
|
// public List<Subsystem> subsystems = new ArrayList<>();
|
||||||
|
private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
|
||||||
|
|
||||||
// ! Teleop Commands
|
private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d());
|
||||||
public void stop() {
|
// ! Teleop Commands
|
||||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
public void stop() {
|
||||||
m_robotSwerveDrive.stopModules();
|
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||||
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
m_robotSwerveDrive.stopModules();
|
||||||
}
|
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
||||||
|
}
|
||||||
|
|
||||||
// ! /* Autos */
|
// ! /* Autos */
|
||||||
private SendableChooser<String> autoChooser;
|
private SendableChooser<String> autoChooser;
|
||||||
private Command autoCommand;
|
private Command autoCommand;
|
||||||
|
|
||||||
|
|
||||||
private Command IntakeExtended = new SequentialCommandGroup(
|
private Command IntakeExtended = new SequentialCommandGroup(
|
||||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
|
||||||
);
|
);
|
||||||
|
|
||||||
// private Command LidarIntake = new SequentialCommandGroup(
|
// private Command LidarIntake = new SequentialCommandGroup(
|
||||||
// // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
|
// // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
|
||||||
// // RobotIntakeDown,
|
// // RobotIntakeDown,
|
||||||
// // new WaitCommand(1),
|
// // new WaitCommand(1),
|
||||||
// // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
|
// // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
|
||||||
// new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
|
// new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
|
||||||
|
|
||||||
// // new RunCommand(
|
// // new RunCommand(
|
||||||
// // () -> m_robotSwerveDrive.driveWithInput(
|
// // () -> m_robotSwerveDrive.driveWithInput(
|
||||||
// // m_lidar.getClosestBall(),
|
// // m_lidar.getClosestBall(),
|
||||||
// // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
|
// // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
|
||||||
// // false
|
// // false
|
||||||
// // ),
|
// // ),
|
||||||
// // m_robotSwerveDrive
|
// // m_robotSwerveDrive
|
||||||
// // )
|
// // )
|
||||||
// // .withTimeout(5.0)
|
// // .withTimeout(5.0)
|
||||||
// // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
|
// // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
|
||||||
// );
|
// );
|
||||||
|
|
||||||
private Command RobotRev = new SequentialCommandGroup(
|
private Command RobotRev = new SequentialCommandGroup(
|
||||||
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
|
new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
|
||||||
IntakeExtended,
|
IntakeExtended,
|
||||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
|
||||||
);
|
);
|
||||||
|
|
||||||
private Command IntakeRetracted = new SequentialCommandGroup(
|
private Command RobotShootDriving = new SequentialCommandGroup(
|
||||||
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
|
new RunCommand(() ->
|
||||||
);
|
m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION)
|
||||||
|
).withTimeout(20)
|
||||||
|
).finallyDo((interrupted) ->
|
||||||
|
m_robotSwerveDrive.disableRotationOverride()
|
||||||
|
);
|
||||||
|
|
||||||
private Command RobotShoot = new SequentialCommandGroup(
|
private Command IntakeRetracted = new SequentialCommandGroup(
|
||||||
// TEST NEW AUTO ALIGN
|
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
|
||||||
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
|
);
|
||||||
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
|
||||||
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
private Command RobotShoot = new SequentialCommandGroup(
|
||||||
new WaitCommand(2),
|
// TEST NEW AUTO ALIGN
|
||||||
IntakeRetracted,
|
//new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false),
|
||||||
new WaitCommand(5),
|
new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
|
||||||
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
|
||||||
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
new WaitCommand(5),
|
||||||
);
|
IntakeRetracted,
|
||||||
|
new WaitCommand(10),
|
||||||
|
new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
|
||||||
|
new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
public RobotContainer() {
|
public RobotContainer() {
|
||||||
|
|
||||||
configureButtonBindings();
|
configureSINGLEBindings();
|
||||||
|
|
||||||
// Called on first robot enable
|
// Called on first robot enable
|
||||||
DeferredBlock.addBlock(() -> {
|
DeferredBlock.addBlock(() -> {
|
||||||
m_robotSwerveDrive.resetGyro();
|
m_robotSwerveDrive.resetGyro();
|
||||||
}, false);
|
}, false);
|
||||||
|
|
||||||
// Called on every robot enable
|
// Called on every robot enable
|
||||||
DeferredBlock.addBlock(() -> {
|
DeferredBlock.addBlock(() -> {
|
||||||
TimesNegativeOne.update();
|
TimesNegativeOne.update();
|
||||||
FieldPositions.update();
|
FieldPositions.update();
|
||||||
|
|
||||||
m_robotIntake.io.updateGains();
|
|
||||||
m_robotShooter.io.updateGains();
|
|
||||||
}, true);
|
|
||||||
|
|
||||||
NamedCommands.registerCommand("Robot Rev Up", RobotRev);
|
|
||||||
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
|
|
||||||
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
|
|
||||||
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
|
|
||||||
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
|
|
||||||
|
|
||||||
|
|
||||||
DriverStation.silenceJoystickConnectionWarning(true);
|
|
||||||
|
|
||||||
// Drive normally
|
|
||||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
|
||||||
m_robotSwerveDrive.driveWithInput(
|
|
||||||
getDeadbandedDriverController().getLeft(),
|
|
||||||
getDeadbandedDriverController().getRight(),true);
|
|
||||||
|
|
||||||
}, m_robotSwerveDrive)
|
|
||||||
.withName("SwerveDrive DefaultCommand"));
|
|
||||||
|
|
||||||
m_robotSwerveDrive.setToSlow();
|
|
||||||
|
|
||||||
makeAutoChooser();
|
|
||||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Use this method to define your button->command mappings. Buttons can be
|
|
||||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
|
||||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
|
||||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
|
||||||
*/
|
|
||||||
private void configureButtonBindings() {
|
|
||||||
|
|
||||||
//Driver controls
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
|
||||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
|
||||||
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
|
||||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
|
||||||
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
|
||||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
|
||||||
.onTrue(new InstantCommand(() -> {
|
|
||||||
m_robotIntake.io.updateGains();
|
m_robotIntake.io.updateGains();
|
||||||
m_robotShooter.io.updateGains();
|
m_robotShooter.io.updateGains();
|
||||||
|
}, true);
|
||||||
|
|
||||||
|
NamedCommands.registerCommand("Robot Rev Up", RobotRev);
|
||||||
|
NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
|
||||||
|
NamedCommands.registerCommand("Robot Shoot", RobotShoot);
|
||||||
|
// NamedCommands.registerCommand("Lidar Intake", LidarIntake);
|
||||||
|
NamedCommands.registerCommand("Intake Extended", IntakeExtended);
|
||||||
|
NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
|
||||||
|
|
||||||
|
NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed));
|
||||||
|
NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter));
|
||||||
|
NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter));
|
||||||
|
NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter));
|
||||||
|
NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter));
|
||||||
|
|
||||||
|
NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> {
|
||||||
|
if (TimesNegativeOne.isRed) {
|
||||||
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
|
||||||
|
} else {
|
||||||
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
|
||||||
|
}
|
||||||
|
}));
|
||||||
|
|
||||||
|
NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> {
|
||||||
|
if (!TimesNegativeOne.isRed) {
|
||||||
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
|
||||||
|
} else {
|
||||||
|
m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
|
||||||
|
}
|
||||||
}));
|
}));
|
||||||
|
|
||||||
|
|
||||||
// IF the driver is holding the left trigger, intake driving
|
DriverStation.silenceJoystickConnectionWarning(true);
|
||||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
|
||||||
.whileTrue(new RunCommand(
|
|
||||||
() -> {
|
|
||||||
m_robotSwerveDrive.driveIntakeOrientation(
|
|
||||||
getDeadbandedDriverController().getLeft(),
|
|
||||||
getDeadbandedDriverController().getRight()
|
|
||||||
|
|
||||||
);
|
// Drive normally
|
||||||
}, m_robotSwerveDrive))
|
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||||
|
m_robotSwerveDrive.driveWithInput(
|
||||||
|
getDeadbandedDriverController().getLeft(),
|
||||||
|
getDeadbandedDriverController().getRight(),true);
|
||||||
|
|
||||||
|
}, m_robotSwerveDrive)
|
||||||
|
.withName("SwerveDrive DefaultCommand"));
|
||||||
|
|
||||||
|
m_robotSwerveDrive.setToSlow();
|
||||||
|
|
||||||
|
makeAutoChooser();
|
||||||
|
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Use this method to define your button->command mappings. Buttons can be
|
||||||
|
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||||
|
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||||
|
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||||
|
*/
|
||||||
|
private void configureButtonBindings() {
|
||||||
|
|
||||||
|
//Driver controls
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotIntake.io.updateGains();
|
||||||
|
m_robotShooter.io.updateGains();
|
||||||
|
}));
|
||||||
|
|
||||||
|
|
||||||
|
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
||||||
|
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotSwerveDrive.setToSlow();
|
||||||
|
m_robotSwerveDrive.shiftUpRot();
|
||||||
|
}))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotSwerveDrive.setToFast();
|
||||||
|
m_robotSwerveDrive.shiftDownRot();
|
||||||
|
}));
|
||||||
|
|
||||||
|
//TEST - > X positino on wheels
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||||
|
.whileTrue(new RunCommand(() -> {
|
||||||
|
m_robotSwerveDrive.defenseXPosition();
|
||||||
|
}, m_robotSwerveDrive))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotSwerveDrive.stopDefenseXPosition();
|
||||||
|
}));
|
||||||
|
|
||||||
|
//TEST - > PID positinon
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
currentPose = m_robotSwerveDrive.getCurrentPose();
|
||||||
|
}))
|
||||||
|
.whileTrue(new RunCommand(() -> {
|
||||||
|
m_stayInPosition.goToTargetPose(currentPose);
|
||||||
|
}, m_robotSwerveDrive))
|
||||||
.onFalse(new InstantCommand(() -> {
|
.onFalse(new InstantCommand(() -> {
|
||||||
m_robotSwerveDrive.softStop();
|
m_robotSwerveDrive.softStop();
|
||||||
|
|
||||||
}));
|
}));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
||||||
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
||||||
.whileTrue(new RunCommand(
|
.onTrue(new InstantCommand(() -> {
|
||||||
() -> {
|
m_robotSwerveDrive.setToSlow();
|
||||||
|
}))
|
||||||
|
.whileTrue(new RunCommand(() -> {
|
||||||
m_robotSwerveDrive.driveFacingPosition(
|
m_robotSwerveDrive.driveFacingPosition(
|
||||||
getDeadbandedDriverController().getLeft(),
|
getDeadbandedDriverController().getLeft(),
|
||||||
FieldPositions.HUB_POSITION,
|
FieldPositions.HUB_POSITION,
|
||||||
@@ -326,6 +391,126 @@ public class RobotContainer {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
private void configureSINGLEBindings() {
|
||||||
|
|
||||||
|
//Driver controls
|
||||||
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||||
|
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// TEST-> the driver is holding the left trigger, drive slow and rotation up
|
||||||
|
// new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||||
|
// .onTrue(new InstantCommand(() -> {
|
||||||
|
// m_robotSwerveDrive.setToSlow();
|
||||||
|
// m_robotSwerveDrive.shiftUpRot();
|
||||||
|
// }))
|
||||||
|
// .onFalse(new InstantCommand(() -> {
|
||||||
|
// m_robotSwerveDrive.setToFast();
|
||||||
|
// m_robotSwerveDrive.shiftDownRot();
|
||||||
|
// }));
|
||||||
|
|
||||||
|
//TEST - > X positino on wheels
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||||
|
.whileTrue(new RunCommand(() -> {
|
||||||
|
m_robotSwerveDrive.defenseXPosition();
|
||||||
|
}, m_robotSwerveDrive))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotSwerveDrive.stopDefenseXPosition();
|
||||||
|
}));
|
||||||
|
|
||||||
|
//TEST - > PID positinon
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
currentPose = m_robotSwerveDrive.getCurrentPose();
|
||||||
|
}))
|
||||||
|
.whileTrue(new RunCommand(() -> {
|
||||||
|
m_stayInPosition.goToTargetPose(currentPose);
|
||||||
|
}, m_robotSwerveDrive))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotSwerveDrive.softStop();
|
||||||
|
|
||||||
|
}));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
|
||||||
|
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotSwerveDrive.setToSlow();
|
||||||
|
}))
|
||||||
|
.whileTrue(new RunCommand(() -> {
|
||||||
|
m_robotSwerveDrive.driveFacingPosition(
|
||||||
|
getDeadbandedDriverController().getLeft(),
|
||||||
|
FieldPositions.HUB_POSITION,
|
||||||
|
ShooterConstants.AIM_LEAD_TIME.get()
|
||||||
|
);
|
||||||
|
}, m_robotSwerveDrive)
|
||||||
|
);
|
||||||
|
|
||||||
|
// D-PAD fine alignment
|
||||||
|
new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
|
||||||
|
.whileTrue(new RunCommand(
|
||||||
|
() -> m_robotSwerveDrive.driveFine(
|
||||||
|
new Translation2d(
|
||||||
|
1,
|
||||||
|
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
|
||||||
|
),
|
||||||
|
getDeadbandedDriverController().getRight(), 0.15
|
||||||
|
), m_robotSwerveDrive))
|
||||||
|
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
|
||||||
|
|
||||||
|
|
||||||
|
//allow shooting with right trigger
|
||||||
|
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotShooter.allowShooting();
|
||||||
|
})).onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotShooter.denyShooting();
|
||||||
|
}));
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//set shooter ready (rev) with left trigger hold
|
||||||
|
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotIntake.setMode(IntakeMode.Idle);
|
||||||
|
m_robotIntake.rollerStop();
|
||||||
|
m_robotShooter.spinUpShooting();
|
||||||
|
}))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotShooter.spinUpIdle();
|
||||||
|
}));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotIntake.setMode(IntakeMode.Extending);
|
||||||
|
}))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotIntake.setMode(IntakeMode.Idle);
|
||||||
|
}));
|
||||||
|
|
||||||
|
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||||
|
.onTrue(new InstantCommand(() -> {
|
||||||
|
m_robotIntake.setMode(IntakeMode.Retracting);
|
||||||
|
}))
|
||||||
|
.onFalse(new InstantCommand(() -> {
|
||||||
|
m_robotIntake.setMode(IntakeMode.Idle);
|
||||||
|
}));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
|
//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -25,6 +25,7 @@ import frc4388.robot.subsystems.intake.IntakeReal;
|
|||||||
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
import frc4388.robot.subsystems.shooter.ShooterConstants;
|
||||||
import frc4388.robot.subsystems.shooter.ShooterIO;
|
import frc4388.robot.subsystems.shooter.ShooterIO;
|
||||||
import frc4388.robot.subsystems.shooter.ShooterReal;
|
import frc4388.robot.subsystems.shooter.ShooterReal;
|
||||||
|
import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
|
||||||
// import frc4388.robot.subsystems.elevator.ElevatorIO;
|
// import frc4388.robot.subsystems.elevator.ElevatorIO;
|
||||||
// import frc4388.robot.subsystems.elevator.ElevatorReal;
|
// import frc4388.robot.subsystems.elevator.ElevatorReal;
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
|
||||||
@@ -133,8 +134,15 @@ public class RobotMap {
|
|||||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
|
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
// case SIM:
|
case SIM:
|
||||||
// break;
|
leftCamera = new VisionIO() {};
|
||||||
|
rightCamera = new VisionIO() {};
|
||||||
|
|
||||||
|
swerveDrivetrain = new SimpleSwerveSim() {};
|
||||||
|
|
||||||
|
shooterIO = new ShooterIO() {};
|
||||||
|
intakeIO = new IntakeIO() {};
|
||||||
|
break;
|
||||||
default:
|
default:
|
||||||
leftCamera = new VisionIO() {};
|
leftCamera = new VisionIO() {};
|
||||||
rightCamera = new VisionIO() {};
|
rightCamera = new VisionIO() {};
|
||||||
|
|||||||
@@ -1,49 +0,0 @@
|
|||||||
package frc4388.robot.commands;
|
|
||||||
|
|
||||||
import java.time.Instant;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
|
|
||||||
// Command to repeat a joystick movement for a specific time.
|
|
||||||
public class MoveForTimeCommand extends Command {
|
|
||||||
private final SwerveDrive swerveDrive;
|
|
||||||
private final Translation2d leftStick;
|
|
||||||
private final Translation2d rightStick;
|
|
||||||
private final long duration;
|
|
||||||
private final boolean robotRelative;
|
|
||||||
|
|
||||||
private Instant startTime;
|
|
||||||
|
|
||||||
public MoveForTimeCommand(
|
|
||||||
SwerveDrive swerveDrive,
|
|
||||||
Translation2d leftStick,
|
|
||||||
Translation2d rightStick,
|
|
||||||
long millis,
|
|
||||||
boolean robotRelative) {
|
|
||||||
|
|
||||||
addRequirements(swerveDrive);
|
|
||||||
|
|
||||||
this.swerveDrive = swerveDrive;
|
|
||||||
this.leftStick = leftStick;
|
|
||||||
this.rightStick = rightStick;
|
|
||||||
this.duration = millis;
|
|
||||||
this.robotRelative = robotRelative;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
startTime = Instant.now();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,45 +0,0 @@
|
|||||||
package frc4388.robot.commands;
|
|
||||||
|
|
||||||
import java.util.function.Supplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
|
|
||||||
// Command to repeat a joystick movement for a specific time.
|
|
||||||
public class MoveUntilSuply extends Command {
|
|
||||||
private final SwerveDrive swerveDrive;
|
|
||||||
private final Translation2d leftStick;
|
|
||||||
private final Translation2d rightStick;
|
|
||||||
private final Supplier<Boolean> truth;
|
|
||||||
private final boolean robotRelative;
|
|
||||||
|
|
||||||
public MoveUntilSuply(
|
|
||||||
SwerveDrive swerveDrive,
|
|
||||||
Translation2d leftStick,
|
|
||||||
Translation2d rightStick,
|
|
||||||
Supplier<Boolean> truth,
|
|
||||||
boolean robotRelative) {
|
|
||||||
addRequirements(swerveDrive);
|
|
||||||
|
|
||||||
this.swerveDrive = swerveDrive;
|
|
||||||
this.leftStick = leftStick;
|
|
||||||
this.rightStick = rightStick;
|
|
||||||
this.truth = truth;
|
|
||||||
this.robotRelative = robotRelative;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return truth.get();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,35 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package frc4388.robot.commands.Swerve;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import frc4388.robot.commands.PID;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
|
|
||||||
public class RotateToAngle extends PID {
|
|
||||||
|
|
||||||
SwerveDrive drive;
|
|
||||||
double targetAngle;
|
|
||||||
|
|
||||||
/** Creates a new RotateToAngle. */
|
|
||||||
public RotateToAngle(SwerveDrive drive, double targetAngle) {
|
|
||||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
|
||||||
|
|
||||||
this.drive = drive;
|
|
||||||
this.targetAngle = targetAngle;
|
|
||||||
|
|
||||||
addRequirements(drive);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public double getError() {
|
|
||||||
return targetAngle - drive.getGyroAngle();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void runWithOutput(double output) {
|
|
||||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,54 @@
|
|||||||
|
// Copyright (c) FIRST and other WPILib contributors.
|
||||||
|
// Open Source Software; you can modify and/or share it under the terms of
|
||||||
|
// the WPILib BSD license file in the root directory of this project.
|
||||||
|
|
||||||
|
package frc4388.robot.commands.Swerve;
|
||||||
|
|
||||||
|
import org.littletonrobotics.junction.Logger;
|
||||||
|
import org.littletonrobotics.junction.AutoLogOutput;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
import edu.wpi.first.math.geometry.Translation2d;
|
||||||
|
import frc4388.robot.commands.PID;
|
||||||
|
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||||
|
|
||||||
|
public class StayInPosition extends PID {
|
||||||
|
SwerveDrive drive;
|
||||||
|
Translation2d driveTranslation = new Translation2d();
|
||||||
|
|
||||||
|
public StayInPosition(SwerveDrive drive) {
|
||||||
|
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||||
|
this.drive = drive;
|
||||||
|
addRequirements(drive);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void goToTargetPose(Pose2d targetPose) {
|
||||||
|
Pose2d currentPose = drive.getCurrentPose();
|
||||||
|
double translationX = targetPose.getX() - currentPose.getX();
|
||||||
|
double translationY = targetPose.getY() - currentPose.getY();
|
||||||
|
if (translationX > 0.2){
|
||||||
|
//If below is changed make this change too so it never goes over 1
|
||||||
|
translationX = 0.2;
|
||||||
|
}
|
||||||
|
if (translationY > 0.2){
|
||||||
|
//Same here
|
||||||
|
translationY = 0.2;
|
||||||
|
}
|
||||||
|
if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) {
|
||||||
|
driveTranslation = new Translation2d();
|
||||||
|
} else {
|
||||||
|
//TODO: Tweak for best corrector against another bot
|
||||||
|
driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5);
|
||||||
|
}
|
||||||
|
|
||||||
|
drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public double getError() {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runWithOutput(double output) {}
|
||||||
|
}
|
||||||
@@ -1,197 +0,0 @@
|
|||||||
package frc4388.robot.commands.Swerve;
|
|
||||||
|
|
||||||
import java.io.FileInputStream;
|
|
||||||
import java.util.ArrayList;
|
|
||||||
import java.util.function.Supplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
import frc4388.utility.compute.DataUtils;
|
|
||||||
import frc4388.utility.controller.VirtualController;
|
|
||||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
|
||||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s
|
|
||||||
* @author Zachary Wilke
|
|
||||||
*/
|
|
||||||
public class neoJoystickPlayback extends Command {
|
|
||||||
private final SwerveDrive swerve;
|
|
||||||
private final VirtualController[] controllers;
|
|
||||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
|
||||||
private final Supplier<String> filenameGetter;
|
|
||||||
private String filename;
|
|
||||||
private int frame_index = 0;
|
|
||||||
// private long startTime = 0;
|
|
||||||
// private long playbackTime = 0;
|
|
||||||
private boolean m_finished = false; // ! There is no better way.
|
|
||||||
private boolean m_shouldfree = false; // should free memory on ending
|
|
||||||
|
|
||||||
private byte m_numAxes = 0;
|
|
||||||
private byte m_numPOVs = 0;
|
|
||||||
private byte m_numControllers = 0;
|
|
||||||
private short m_numFrames = -1;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
|
||||||
* @param swerve m_robotSwerveDrive
|
|
||||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
|
||||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
|
||||||
* @param shouldfree Unloads the auto on compleation or intruption.
|
|
||||||
* @param instantload Load the auto on object instantiation
|
|
||||||
*/
|
|
||||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
|
||||||
this.swerve = swerve;
|
|
||||||
this.filenameGetter = filenameGetter;
|
|
||||||
this.controllers = controllers;
|
|
||||||
this.m_shouldfree = shouldfree;
|
|
||||||
|
|
||||||
if (instantload) loadAuto();
|
|
||||||
addRequirements(this.swerve);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
|
||||||
* @param swerve m_robotSwerveDrive
|
|
||||||
* @param filename a String containing the name of the auto file you wish to playback.
|
|
||||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
|
||||||
* @param shouldfree unloads the auto on compleation or intruption.
|
|
||||||
* @param instantload load the auto on object instantiation
|
|
||||||
*/
|
|
||||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
|
||||||
this(swerve, () -> filename, controllers, shouldfree, instantload);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
|
||||||
* @param swerve m_robotSwerveDrive
|
|
||||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
|
||||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
|
||||||
*/
|
|
||||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
|
|
||||||
this(swerve, filenameGetter, controllers, true, false);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
|
||||||
* @param swerve m_robotSwerveDrive
|
|
||||||
* @param filename a String containing the name of the auto file you wish to playback.
|
|
||||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
|
||||||
*/
|
|
||||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
|
|
||||||
this(swerve, () -> filename, controllers, true, false);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Load the auto file from disk into memory
|
|
||||||
* @return Returns true if loading was successful, else wise; return false
|
|
||||||
* @implNote if the auto is already loaded, it will return true.
|
|
||||||
*/
|
|
||||||
public boolean loadAuto() {
|
|
||||||
filename = filenameGetter.get();
|
|
||||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
|
|
||||||
if (m_numFrames != -1 && m_numFrames == frames.size()) {
|
|
||||||
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
m_numAxes = stream.readNBytes(1)[0];
|
|
||||||
m_numPOVs = stream.readNBytes(1)[0];
|
|
||||||
m_numControllers = stream.readNBytes(1)[0];
|
|
||||||
m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
|
||||||
|
|
||||||
if (m_numControllers > controllers.length) {
|
|
||||||
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
|
|
||||||
+ " virtual controllers but only " + controllers.length + " were given");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < m_numFrames; i++) {
|
|
||||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
|
||||||
for (int j = 0; j < m_numControllers; j++) {
|
|
||||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
|
||||||
double[] axes = new double[m_numAxes];
|
|
||||||
for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
|
|
||||||
axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
|
||||||
}
|
|
||||||
short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
|
||||||
short[] POV = new short[m_numPOVs];
|
|
||||||
for (int k = 0; k < m_numPOVs; k++) {
|
|
||||||
POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
|
||||||
}
|
|
||||||
controllerFrame.axes = axes;
|
|
||||||
controllerFrame.button = button;
|
|
||||||
controllerFrame.POV = POV;
|
|
||||||
frame.controllerFrames[j] = controllerFrame;
|
|
||||||
}
|
|
||||||
frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
|
|
||||||
frames.add(frame);
|
|
||||||
}
|
|
||||||
|
|
||||||
System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
|
|
||||||
return true;
|
|
||||||
|
|
||||||
} catch (Exception e) {
|
|
||||||
e.printStackTrace();
|
|
||||||
System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Unloads the auto.
|
|
||||||
*/
|
|
||||||
public void unloadAuto() {
|
|
||||||
System.out.println("AUTOPLAYBACK: Auto unloaded");
|
|
||||||
frames.clear();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
// startTime = System.currentTimeMillis();
|
|
||||||
// playbackTime = 0;
|
|
||||||
frame_index = 0;
|
|
||||||
|
|
||||||
m_finished = !loadAuto();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
if (frame_index >= m_numFrames) m_finished = true;
|
|
||||||
if (m_finished) return;
|
|
||||||
|
|
||||||
// if (frame_index == 0) {
|
|
||||||
// startTime = System.currentTimeMillis();
|
|
||||||
// playbackTime = 0;
|
|
||||||
// } else {
|
|
||||||
// playbackTime = System.currentTimeMillis() - startTime;
|
|
||||||
// }
|
|
||||||
|
|
||||||
AutoRecordingFrame frame = frames.get(frame_index);
|
|
||||||
for (int i = 0; i < controllers.length; i++) {
|
|
||||||
AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
|
|
||||||
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
|
|
||||||
if (i == 0) {
|
|
||||||
this.swerve.driveWithInput(
|
|
||||||
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
|
|
||||||
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
|
|
||||||
true);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
frame_index++;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
for (VirtualController controller : controllers) controller.zeroControls();
|
|
||||||
swerve.stopModules();
|
|
||||||
if (m_shouldfree) unloadAuto();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return m_finished;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,129 +0,0 @@
|
|||||||
package frc4388.robot.commands.Swerve;
|
|
||||||
|
|
||||||
import java.io.FileOutputStream;
|
|
||||||
import java.util.ArrayList;
|
|
||||||
import java.util.function.Supplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj.XboxController;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
import frc4388.utility.compute.DataUtils;
|
|
||||||
import frc4388.utility.controller.DeadbandedXboxController;
|
|
||||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
|
|
||||||
import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
|
|
||||||
* @author Zachary Wilke
|
|
||||||
*/
|
|
||||||
public class neoJoystickRecorder extends Command {
|
|
||||||
private final SwerveDrive swerve;
|
|
||||||
private final XboxController[] controllers;
|
|
||||||
private String filename;
|
|
||||||
private final Supplier<String> filenameGetter;
|
|
||||||
private long startTime = -1;
|
|
||||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
|
||||||
* @param swerve m_robotSwerveDrive
|
|
||||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
|
||||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
|
||||||
*/
|
|
||||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
|
|
||||||
this.swerve = swerve;
|
|
||||||
this.controllers = controllers;
|
|
||||||
this.filenameGetter = filenameGetter;
|
|
||||||
this.filename = "";
|
|
||||||
|
|
||||||
addRequirements(this.swerve);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
|
||||||
* @param swerve m_robotSwerveDrive
|
|
||||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
|
||||||
* @param filename a String containing the name of the auto file you wish to playback.
|
|
||||||
*/
|
|
||||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
|
|
||||||
this(swerve, controllers, () -> filename);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
frames.clear();
|
|
||||||
|
|
||||||
this.startTime = System.currentTimeMillis();
|
|
||||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
|
||||||
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
|
|
||||||
frames.add(frame);
|
|
||||||
this.filename = this.filenameGetter.get();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
System.out.println("AUTORECORD: RECORDING");
|
|
||||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
|
||||||
frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
|
|
||||||
for (int i = 0; i < controllers.length; i++) {
|
|
||||||
XboxController controller = controllers[i];
|
|
||||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
|
||||||
double[] axes = {controller.getLeftX(), controller.getLeftY(),
|
|
||||||
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
|
|
||||||
controller.getRightX(), controller.getRightY()};
|
|
||||||
short button = 0;
|
|
||||||
for (int j = 0; j < 10; j++)
|
|
||||||
if (controller.getRawButton(j+1))
|
|
||||||
button |= 1 << j;
|
|
||||||
short[] POV = {(short)(controller.getPOV())};
|
|
||||||
controllerFrame.axes = axes;
|
|
||||||
controllerFrame.button = button;
|
|
||||||
controllerFrame.POV = POV;
|
|
||||||
frame.controllerFrames[i] = controllerFrame;
|
|
||||||
}
|
|
||||||
|
|
||||||
frames.add(frame);
|
|
||||||
|
|
||||||
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
|
|
||||||
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
|
|
||||||
true); // Really jank way of doing this.
|
|
||||||
|
|
||||||
}
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
|
|
||||||
// header: size of 0x5
|
|
||||||
// byte Number of axes per controller
|
|
||||||
// byte Number of POVs per controller
|
|
||||||
// byte Number of controllers
|
|
||||||
// short Number of frames
|
|
||||||
stream.write(new byte[]{6, 1, (byte) controllers.length});
|
|
||||||
stream.write(DataUtils.shortToByteArray((short) frames.size()));
|
|
||||||
|
|
||||||
// frame
|
|
||||||
// controller frame * number of controllers
|
|
||||||
// int unix time stamp.
|
|
||||||
for (AutoRecordingFrame frame : frames) {
|
|
||||||
// controller frame
|
|
||||||
// double axis * Number of axes per controller
|
|
||||||
// short button states
|
|
||||||
// short POV * Number of POVs per controller
|
|
||||||
for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
|
|
||||||
for (double axis: controllerFrame.axes) {
|
|
||||||
stream.write(DataUtils.doubleToByteArray(axis));
|
|
||||||
}
|
|
||||||
stream.write(DataUtils.shortToByteArray(controllerFrame.button));
|
|
||||||
for (short POV: controllerFrame.POV) {
|
|
||||||
stream.write(DataUtils.shortToByteArray(POV));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
stream.write(DataUtils.intToByteArray(frame.timeStamp));
|
|
||||||
}
|
|
||||||
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
|
|
||||||
} catch (Exception e) {
|
|
||||||
e.printStackTrace();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,104 +0,0 @@
|
|||||||
package frc4388.robot.commands;
|
|
||||||
|
|
||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
|
|
||||||
|
|
||||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
|
|
||||||
import java.util.function.BooleanSupplier;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* A command composition that runs one of two commands, depending on the value of the given
|
|
||||||
* condition when this command is initialized.
|
|
||||||
*
|
|
||||||
* <p>The rules for command compositions apply: command instances that are passed to it cannot be
|
|
||||||
* added to any other composition or scheduled individually, and the composition requires all
|
|
||||||
* subsystems its components require.
|
|
||||||
*
|
|
||||||
* <p>This class is provided by the NewCommands VendorDep
|
|
||||||
*/
|
|
||||||
public class WhileTrueCommand extends Command {
|
|
||||||
private final Command m_whileTrue;
|
|
||||||
private final BooleanSupplier m_condition;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Creates a new WhileTrueCommand.
|
|
||||||
*
|
|
||||||
* @param whileTrue the command to run while the condition is true
|
|
||||||
* @param condition the condition to determine which command to run
|
|
||||||
*/
|
|
||||||
@SuppressWarnings("this-escape")
|
|
||||||
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
|
|
||||||
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
|
|
||||||
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
|
|
||||||
|
|
||||||
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
|
|
||||||
|
|
||||||
// addRequirements(whileTrue.getRequirements());
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
if(m_condition.getAsBoolean())
|
|
||||||
m_whileTrue.initialize();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
m_whileTrue.execute();
|
|
||||||
|
|
||||||
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
|
|
||||||
|
|
||||||
if(!m_whileTrue.isFinished())
|
|
||||||
return;
|
|
||||||
|
|
||||||
if(m_condition.getAsBoolean()){
|
|
||||||
m_whileTrue.end(false);
|
|
||||||
m_whileTrue.initialize();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {
|
|
||||||
m_whileTrue.end(interrupted);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean runsWhenDisabled() {
|
|
||||||
return m_whileTrue.runsWhenDisabled();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public InterruptionBehavior getInterruptionBehavior() {
|
|
||||||
if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
|
|
||||||
return InterruptionBehavior.kCancelSelf;
|
|
||||||
} else {
|
|
||||||
return InterruptionBehavior.kCancelIncoming;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initSendable(SendableBuilder builder) {
|
|
||||||
super.initSendable(builder);
|
|
||||||
builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
|
|
||||||
builder.addStringProperty(
|
|
||||||
"selected",
|
|
||||||
() -> {
|
|
||||||
if (m_whileTrue == null) {
|
|
||||||
return "null";
|
|
||||||
} else {
|
|
||||||
return m_whileTrue.getName();
|
|
||||||
}
|
|
||||||
},
|
|
||||||
null);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
|||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
import frc4388.robot.constants.Constants.AutoConstants;
|
import frc4388.robot.constants.Constants.AutoConstants;
|
||||||
import frc4388.robot.subsystems.Lidar;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||||
|
import frc4388.robot.subsystems.vision.Lidar;
|
||||||
import frc4388.robot.subsystems.vision.Vision;
|
import frc4388.robot.subsystems.vision.Vision;
|
||||||
import frc4388.utility.compute.FieldPositions;
|
import frc4388.utility.compute.FieldPositions;
|
||||||
import frc4388.utility.structs.Gains;
|
import frc4388.utility.structs.Gains;
|
||||||
|
|||||||
@@ -1,37 +0,0 @@
|
|||||||
package frc4388.robot.commands.alignment;
|
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
||||||
|
|
||||||
public class RotTo45 extends Command {
|
|
||||||
|
|
||||||
SwerveDrive m_SwerveDrive;
|
|
||||||
Rotation2d targetAngle;
|
|
||||||
|
|
||||||
|
|
||||||
public RotTo45(SwerveDrive swerveDrive) {
|
|
||||||
m_SwerveDrive = swerveDrive;
|
|
||||||
|
|
||||||
addRequirements(swerveDrive);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
targetAngle = new Rotation2d();
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void execute() {
|
|
||||||
m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
|
|
||||||
// TODO: Tune
|
|
||||||
return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@@ -1,36 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package frc4388.robot.commands.wait;
|
|
||||||
|
|
||||||
import java.util.function.Supplier;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
|
||||||
|
|
||||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
|
||||||
public class waitSupplier extends Command {
|
|
||||||
/** Creates a new waitSupplier. */
|
|
||||||
private final Supplier<Boolean> truth;
|
|
||||||
public waitSupplier(Supplier<Boolean> truth) {
|
|
||||||
this.truth = truth;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Called when the command is initially scheduled.
|
|
||||||
@Override
|
|
||||||
public void initialize() {}
|
|
||||||
|
|
||||||
// Called every time the scheduler runs while the command is scheduled.
|
|
||||||
@Override
|
|
||||||
public void execute() {}
|
|
||||||
|
|
||||||
// Called once the command ends or is interrupted.
|
|
||||||
@Override
|
|
||||||
public void end(boolean interrupted) {}
|
|
||||||
|
|
||||||
// Returns true when the command should end.
|
|
||||||
@Override
|
|
||||||
public boolean isFinished() {
|
|
||||||
return truth.get();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
|||||||
public static final String MAVEN_GROUP = "";
|
public static final String MAVEN_GROUP = "";
|
||||||
public static final String MAVEN_NAME = "2026KPopRobotHunters";
|
public static final String MAVEN_NAME = "2026KPopRobotHunters";
|
||||||
public static final String VERSION = "unspecified";
|
public static final String VERSION = "unspecified";
|
||||||
public static final int GIT_REVISION = 147;
|
public static final int GIT_REVISION = 182;
|
||||||
public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6";
|
public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5";
|
||||||
public static final String GIT_DATE = "2026-03-07 20:08:15 MST";
|
public static final String GIT_DATE = "2026-03-21 13:38:34 MDT";
|
||||||
public static final String GIT_BRANCH = "PikesPeak";
|
public static final String GIT_BRANCH = "MiraOrg";
|
||||||
public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT";
|
public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT";
|
||||||
public static final long BUILD_UNIX_TIME = 1773001639109L;
|
public static final long BUILD_UNIX_TIME = 1774129559544L;
|
||||||
public static final int DIRTY = 1;
|
public static final int DIRTY = 1;
|
||||||
|
|
||||||
private BuildConstants(){}
|
private BuildConstants(){}
|
||||||
|
|||||||
@@ -117,6 +117,11 @@ public final class Constants {
|
|||||||
// Operator ready to shoot, but the driver conditions are bad
|
// Operator ready to shoot, but the driver conditions are bad
|
||||||
public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
|
public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
|
||||||
|
|
||||||
|
// LED color for when the indexer was stuck on ball and going in reverse
|
||||||
|
public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET;
|
||||||
|
|
||||||
|
// LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled
|
||||||
|
public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED;
|
||||||
|
|
||||||
public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
|
public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
|
||||||
// public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
|
// public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
|
||||||
|
|||||||
@@ -1,18 +1,27 @@
|
|||||||
package frc4388.robot.constants;
|
package frc4388.robot.constants;
|
||||||
|
|
||||||
import java.util.Arrays;
|
import static edu.wpi.first.units.Units.Meters;
|
||||||
|
|
||||||
import edu.wpi.first.apriltag.AprilTag;
|
|
||||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||||
import edu.wpi.first.apriltag.AprilTagFields;
|
import edu.wpi.first.apriltag.AprilTagFields;
|
||||||
import edu.wpi.first.math.geometry.Pose3d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.math.geometry.Rotation3d;
|
import edu.wpi.first.math.geometry.Transform2d;
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
|
||||||
import edu.wpi.first.math.geometry.Translation3d;
|
|
||||||
|
|
||||||
public final class FieldConstants {
|
public final class FieldConstants {
|
||||||
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
|
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
|
||||||
|
|
||||||
|
public static final double BUMP_OFFSET = 0.4;
|
||||||
|
public static final Transform2d BUMP_OFFSET_RED = new Transform2d(
|
||||||
|
Meters.of(BUMP_OFFSET),
|
||||||
|
Meters.of(0),
|
||||||
|
new Rotation2d()
|
||||||
|
);
|
||||||
|
public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d(
|
||||||
|
Meters.of(-BUMP_OFFSET),
|
||||||
|
Meters.of(0),
|
||||||
|
new Rotation2d()
|
||||||
|
);
|
||||||
|
|
||||||
// Test april tag field layout
|
// Test april tag field layout
|
||||||
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
|
// public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
|
||||||
// Arrays.asList(new AprilTag[] {
|
// Arrays.asList(new AprilTag[] {
|
||||||
|
|||||||
@@ -45,6 +45,13 @@ public class Intake extends SubsystemBase {
|
|||||||
io.setRollerOutput(state, 0);
|
io.setRollerOutput(state, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public double getRollerTarget() {
|
||||||
|
return state.rollerTargetOutput;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getRollerSpeed() {
|
||||||
|
return state.rollerOutput;
|
||||||
|
}
|
||||||
|
|
||||||
// public enum FieldZone {
|
// public enum FieldZone {
|
||||||
// // The robot should aim at the hub
|
// // The robot should aim at the hub
|
||||||
|
|||||||
+1
-1
@@ -5,7 +5,7 @@
|
|||||||
/* the project. */
|
/* the project. */
|
||||||
/*----------------------------------------------------------------------------*/
|
/*----------------------------------------------------------------------------*/
|
||||||
|
|
||||||
package frc4388.robot.subsystems;
|
package frc4388.robot.subsystems.led;
|
||||||
|
|
||||||
import java.util.concurrent.TimeUnit;
|
import java.util.concurrent.TimeUnit;
|
||||||
|
|
||||||
@@ -9,16 +9,21 @@ import org.littletonrobotics.junction.AutoLogOutput;
|
|||||||
import org.littletonrobotics.junction.Logger;
|
import org.littletonrobotics.junction.Logger;
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
import edu.wpi.first.math.geometry.Translation2d;
|
||||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||||
import edu.wpi.first.units.measure.AngularVelocity;
|
import edu.wpi.first.units.measure.AngularVelocity;
|
||||||
import edu.wpi.first.units.measure.Current;
|
import edu.wpi.first.units.measure.Current;
|
||||||
|
import edu.wpi.first.wpilibj.Timer;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
import frc4388.robot.constants.Constants;
|
import frc4388.robot.constants.Constants;
|
||||||
import frc4388.robot.subsystems.LED;
|
|
||||||
import frc4388.robot.subsystems.intake.Intake;
|
import frc4388.robot.subsystems.intake.Intake;
|
||||||
|
import frc4388.robot.subsystems.led.LED;
|
||||||
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
||||||
import frc4388.utility.compute.FieldPositions;
|
import frc4388.utility.compute.FieldPositions;
|
||||||
|
import frc4388.utility.compute.HubShiftTimer;
|
||||||
|
import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
|
||||||
|
import frc4388.utility.compute.TimesNegativeOne;
|
||||||
|
|
||||||
public class Shooter extends SubsystemBase {
|
public class Shooter extends SubsystemBase {
|
||||||
public ShooterIO io;
|
public ShooterIO io;
|
||||||
@@ -31,6 +36,9 @@ public class Shooter extends SubsystemBase {
|
|||||||
|
|
||||||
// Supplier<Pose2d> m_swervePoseSupplier;
|
// Supplier<Pose2d> m_swervePoseSupplier;
|
||||||
public boolean badShooterVelocity;
|
public boolean badShooterVelocity;
|
||||||
|
public double distanceToHub = 5;
|
||||||
|
public double chassisXSpeed = 0;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
public Shooter(
|
public Shooter(
|
||||||
@@ -76,7 +84,9 @@ public class Shooter extends SubsystemBase {
|
|||||||
this.mode = ShooterMode.Idle;
|
this.mode = ShooterMode.Idle;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public double getDistanceToHub(){
|
||||||
|
return distanceToHub;
|
||||||
|
}
|
||||||
|
|
||||||
public void allowShooting() {
|
public void allowShooting() {
|
||||||
shooterButtonReady = true;
|
shooterButtonReady = true;
|
||||||
@@ -86,6 +96,10 @@ public class Shooter extends SubsystemBase {
|
|||||||
shooterButtonReady = false;
|
shooterButtonReady = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public double getBallVelocity() {
|
||||||
|
return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI;
|
||||||
|
//Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS)
|
||||||
|
}
|
||||||
|
|
||||||
@AutoLogOutput
|
@AutoLogOutput
|
||||||
public ShooterMode getMode() {
|
public ShooterMode getMode() {
|
||||||
@@ -102,40 +116,45 @@ public class Shooter extends SubsystemBase {
|
|||||||
@Override
|
@Override
|
||||||
public void periodic() {
|
public void periodic() {
|
||||||
// FaultReporter.register(this); // TODO Implement fault reporter
|
// FaultReporter.register(this); // TODO Implement fault reporter
|
||||||
|
//Hub Shift logs
|
||||||
|
ShiftInfo info = HubShiftTimer.getShiftInfo();
|
||||||
|
Logger.recordOutput("HubShift/IsActive", info.isActive());
|
||||||
|
Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift());
|
||||||
|
Logger.recordOutput("HubShift/Phase", info.phase().name());
|
||||||
Logger.processInputs("Shooter", state);
|
Logger.processInputs("Shooter", state);
|
||||||
|
|
||||||
io.updateInputs(state);
|
io.updateInputs(state);
|
||||||
|
|
||||||
|
|
||||||
// Get robot positon and speeds
|
// Get robot positon and speeds
|
||||||
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
|
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
|
||||||
double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2));
|
|
||||||
double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI));
|
|
||||||
|
|
||||||
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
|
|
||||||
|
|
||||||
|
|
||||||
// Calculate aim lead
|
|
||||||
// Get the current speed of the robot
|
|
||||||
Translation2d robotSpeed = new Translation2d(
|
Translation2d robotSpeed = new Translation2d(
|
||||||
chassisSpeeds.vxMetersPerSecond,
|
chassisSpeeds.vxMetersPerSecond,
|
||||||
chassisSpeeds.vyMetersPerSecond
|
chassisSpeeds.vyMetersPerSecond
|
||||||
);
|
);
|
||||||
|
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
|
||||||
|
Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle().minus(m_SwerveDrive.getPose2d().getRotation());
|
||||||
|
Pose2d robotPose2d = m_SwerveDrive.getPose2d();
|
||||||
|
|
||||||
// Calculate a point to aim ahead of the actual position.
|
if (TimesNegativeOne.isRed) {
|
||||||
Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
|
chassisXSpeed = chassisSpeeds.vxMetersPerSecond;
|
||||||
|
} else {
|
||||||
|
chassisXSpeed = -chassisSpeeds.vxMetersPerSecond;
|
||||||
|
}
|
||||||
|
|
||||||
|
Translation2d fieldPos = robotPose2d.getTranslation();
|
||||||
// Get the robot's aim distance to hub
|
// Get the robot's aim distance to hub
|
||||||
double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
|
distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm());
|
||||||
|
|
||||||
//Center of hub to cameras in inches
|
//Center of hub to cameras in meters
|
||||||
Logger.recordOutput("Hub Dist", distanceToHub);
|
Logger.recordOutput("Hub Dist", distanceToHub);
|
||||||
|
|
||||||
boolean driverError =
|
boolean driverError =
|
||||||
// XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
|
// XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
|
||||||
// AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
|
// AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
|
||||||
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
|
distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
|
||||||
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
|
distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get() |
|
||||||
|
Math.abs(ang.getDegrees()) > ShooterConstants.AIM_ANGLE.get();
|
||||||
|
|
||||||
|
|
||||||
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
|
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
|
||||||
@@ -145,14 +164,14 @@ public class Shooter extends SubsystemBase {
|
|||||||
|
|
||||||
//revtime calculations
|
//revtime calculations
|
||||||
// double shooterAcceleration =
|
// double shooterAcceleration =
|
||||||
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
|
double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond);
|
||||||
double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
|
double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
|
||||||
// double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
|
// double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
|
||||||
Logger.recordOutput("Time to rev", revTime);
|
Logger.recordOutput("Time to rev", revTime);
|
||||||
|
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case Shooting:
|
case Shooting:
|
||||||
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
|
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
|
||||||
|
|
||||||
int bitmask = (
|
int bitmask = (
|
||||||
(shooterButtonReady ? 1 : 0) +
|
(shooterButtonReady ? 1 : 0) +
|
||||||
@@ -179,13 +198,13 @@ public class Shooter extends SubsystemBase {
|
|||||||
|
|
||||||
case 0b100: // Driver error, button is not pressed
|
case 0b100: // Driver error, button is not pressed
|
||||||
case 0b101: // Driver error, button is pressed
|
case 0b101: // Driver error, button is pressed
|
||||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
io.setIndexerOutput(state, 0);
|
||||||
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
|
m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 0b110: // Driver error, bad shooter vel, button is not pressed
|
case 0b110: // Driver error, bad shooter vel, button is not pressed
|
||||||
case 0b111: // Driver error, bad shooter vel, button is pressed
|
case 0b111: // Driver error, bad shooter vel, button is pressed
|
||||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
io.setIndexerOutput(state, 0);
|
||||||
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
|
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -200,7 +219,7 @@ public class Shooter extends SubsystemBase {
|
|||||||
|
|
||||||
switch (bitmask2) {
|
switch (bitmask2) {
|
||||||
case 0b000: // No errors but button is not pressed
|
case 0b000: // No errors but button is not pressed
|
||||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
io.setIndexerOutput(state, 0);
|
||||||
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
|
m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@@ -211,7 +230,7 @@ public class Shooter extends SubsystemBase {
|
|||||||
|
|
||||||
case 0b010: // Bad shooter velocity, button is not pressed
|
case 0b010: // Bad shooter velocity, button is not pressed
|
||||||
case 0b011: // Bad shooter velocty, button is pressed
|
case 0b011: // Bad shooter velocty, button is pressed
|
||||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
io.setIndexerOutput(state, 0);
|
||||||
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
|
m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@@ -237,12 +256,11 @@ public class Shooter extends SubsystemBase {
|
|||||||
// Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()),
|
// Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()),
|
||||||
// RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
|
// RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
|
||||||
);
|
);
|
||||||
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
|
io.setIndexerOutput(state, 0);
|
||||||
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
|
m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
io.motorStalled(state, m_Intake, m_robotLED);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -15,30 +15,37 @@ import frc4388.utility.status.CanDevice;
|
|||||||
public class ShooterConstants {
|
public class ShooterConstants {
|
||||||
// Motor conversions
|
// Motor conversions
|
||||||
|
|
||||||
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
|
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out -- 0.77 or 1.29
|
||||||
public static final double INDEXER_GEAR_RATIO = 1.;
|
public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31
|
||||||
public static final double T_CONSTANT = 2;
|
public static final double T_CONSTANT = 2;
|
||||||
|
public static final double SHOOTER_RADIUS = 2/39.37;
|
||||||
|
public static final double INDEXER_RADIUS = 0.625/39.37;
|
||||||
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
|
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
|
||||||
public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42);
|
public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42);
|
||||||
// public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
|
// public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
|
||||||
// public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
|
// public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
|
||||||
|
|
||||||
|
|
||||||
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15);
|
public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15);
|
||||||
// public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
|
// public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
|
||||||
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
|
// public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
|
||||||
|
|
||||||
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
|
public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
|
||||||
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
|
public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2);
|
||||||
public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5);
|
public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
|
||||||
|
|
||||||
|
|
||||||
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
|
public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.);
|
||||||
|
public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.);
|
||||||
|
|
||||||
|
|
||||||
|
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1);
|
||||||
|
|
||||||
// Shoot mode tolerances
|
// Shoot mode tolerances
|
||||||
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
|
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
|
||||||
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
|
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
|
||||||
|
|
||||||
|
public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 15);
|
||||||
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
|
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
|
||||||
|
|
||||||
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
|
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
|
||||||
@@ -47,20 +54,23 @@ public class ShooterConstants {
|
|||||||
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3);
|
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3);
|
||||||
|
|
||||||
//
|
//
|
||||||
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
|
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
|
||||||
// Model derived from points
|
double speed = 0;
|
||||||
// double speed =
|
|
||||||
// 1.11576*hubDistMeters*hubDistMeters +
|
|
||||||
// 0.318464*hubDistMeters +
|
|
||||||
// 30.6293;
|
|
||||||
double speed =
|
|
||||||
5.6939*hubDistMeters +
|
|
||||||
22.76545 + MODEL_TRIM.get();
|
|
||||||
|
|
||||||
// double speed =
|
if (Math.abs(chassisXSpeed) < 0.1){
|
||||||
// 0.00610938*hubDistMeters*hubDistMeters
|
speed = 0.0593402*hubDistMeters*hubDistMeters +
|
||||||
// 5.65235*hubDistMeters +
|
4.90561*hubDistMeters +
|
||||||
// 22.82825;
|
30.35696 + MODEL_TRIM.get();
|
||||||
|
} else if (chassisXSpeed > 0){
|
||||||
|
speed = 0.0593402*hubDistMeters*hubDistMeters +
|
||||||
|
4.90561*hubDistMeters +
|
||||||
|
30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
|
||||||
|
|
||||||
|
} else { // Negative is closer to hub
|
||||||
|
speed = 0.0593402*hubDistMeters*hubDistMeters +
|
||||||
|
4.90561*hubDistMeters +
|
||||||
|
30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
|
||||||
|
}
|
||||||
|
|
||||||
double max = SHOOTER_MAX_VELOCITY.get();
|
double max = SHOOTER_MAX_VELOCITY.get();
|
||||||
|
|
||||||
@@ -79,7 +89,7 @@ public class ShooterConstants {
|
|||||||
// Motor Configuration
|
// Motor Configuration
|
||||||
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
|
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
|
||||||
.withKV(0.0)
|
.withKV(0.0)
|
||||||
.withKP(0.08)
|
.withKP(0.02)
|
||||||
.withKI(0.15)
|
.withKI(0.15)
|
||||||
.withKD(0.0);
|
.withKD(0.0);
|
||||||
|
|
||||||
@@ -88,7 +98,7 @@ public class ShooterConstants {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
|
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02);
|
||||||
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
|
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
|
||||||
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
|
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
|
||||||
|
|
||||||
|
|||||||
@@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog;
|
|||||||
|
|
||||||
import edu.wpi.first.units.measure.AngularVelocity;
|
import edu.wpi.first.units.measure.AngularVelocity;
|
||||||
import edu.wpi.first.units.measure.Current;
|
import edu.wpi.first.units.measure.Current;
|
||||||
|
import frc4388.robot.subsystems.intake.Intake;
|
||||||
|
import frc4388.robot.subsystems.led.LED;
|
||||||
|
|
||||||
public interface ShooterIO {
|
public interface ShooterIO {
|
||||||
@AutoLog
|
@AutoLog
|
||||||
@@ -49,6 +51,7 @@ public interface ShooterIO {
|
|||||||
|
|
||||||
public default void updateInputs(ShooterState state) {}
|
public default void updateInputs(ShooterState state) {}
|
||||||
|
|
||||||
|
public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {}
|
||||||
|
|
||||||
public default void updateGains() {}
|
public default void updateGains() {}
|
||||||
}
|
}
|
||||||
@@ -3,11 +3,16 @@ package frc4388.robot.subsystems.shooter;
|
|||||||
import static edu.wpi.first.units.Units.Amps;
|
import static edu.wpi.first.units.Units.Amps;
|
||||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||||
|
|
||||||
|
import org.littletonrobotics.junction.Logger;
|
||||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||||
import com.ctre.phoenix6.hardware.TalonFX;
|
import com.ctre.phoenix6.hardware.TalonFX;
|
||||||
|
|
||||||
import edu.wpi.first.units.measure.AngularVelocity;
|
import edu.wpi.first.units.measure.AngularVelocity;
|
||||||
import edu.wpi.first.units.measure.Current;
|
import edu.wpi.first.units.measure.Current;
|
||||||
|
import edu.wpi.first.wpilibj.Timer;
|
||||||
|
import frc4388.robot.constants.Constants;
|
||||||
|
import frc4388.robot.subsystems.intake.Intake;
|
||||||
|
import frc4388.robot.subsystems.led.LED;
|
||||||
|
|
||||||
public class ShooterReal implements ShooterIO {
|
public class ShooterReal implements ShooterIO {
|
||||||
|
|
||||||
@@ -19,6 +24,14 @@ public class ShooterReal implements ShooterIO {
|
|||||||
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
|
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
|
||||||
// VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
|
// VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
|
||||||
|
|
||||||
|
private final Timer m_stallTimerShooter = new Timer();
|
||||||
|
private final Timer m_stallTimerIndexer = new Timer();
|
||||||
|
private final Timer m_stallTimerRoller = new Timer();
|
||||||
|
private boolean m_shooterStalling = false;
|
||||||
|
private boolean m_indexerStalling = false;
|
||||||
|
private boolean m_rollerStalling = false;
|
||||||
|
public String motorStall = "";
|
||||||
|
|
||||||
|
|
||||||
public ShooterReal(
|
public ShooterReal(
|
||||||
TalonFX shooter1Motor,
|
TalonFX shooter1Motor,
|
||||||
@@ -41,6 +54,59 @@ public class ShooterReal implements ShooterIO {
|
|||||||
// m_indexerVelocity.Slot = 0;
|
// m_indexerVelocity.Slot = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {
|
||||||
|
motorStall = "";
|
||||||
|
if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
|
||||||
|
if (!m_shooterStalling) {
|
||||||
|
m_shooterStalling = true;
|
||||||
|
m_stallTimerShooter.restart();
|
||||||
|
}
|
||||||
|
if (m_stallTimerShooter.hasElapsed(5.0)) {
|
||||||
|
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
|
||||||
|
motorStall = "Shooter Stalled";
|
||||||
|
System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))));
|
||||||
|
System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)));
|
||||||
|
System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
m_shooterStalling = false;
|
||||||
|
m_stallTimerShooter.reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) {
|
||||||
|
if (!m_indexerStalling) {
|
||||||
|
m_indexerStalling = true;
|
||||||
|
m_stallTimerIndexer.restart();
|
||||||
|
}
|
||||||
|
if (m_stallTimerIndexer.hasElapsed(5.0)) {
|
||||||
|
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
|
||||||
|
motorStall = "Indexer Stalled";
|
||||||
|
System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
m_indexerStalling = false;
|
||||||
|
m_stallTimerIndexer.reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) {
|
||||||
|
if (!m_rollerStalling) {
|
||||||
|
m_rollerStalling = true;
|
||||||
|
m_stallTimerRoller.restart();
|
||||||
|
}
|
||||||
|
if (m_stallTimerRoller.hasElapsed(5.0)) {
|
||||||
|
m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
|
||||||
|
motorStall = "Roller Stalled";
|
||||||
|
System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
m_rollerStalling = false;
|
||||||
|
m_stallTimerRoller.reset();
|
||||||
|
}
|
||||||
|
Logger.recordOutput("Stalled Motor: ", motorStall);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
|
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
|
||||||
state.motor1TargetVelocity = target;
|
state.motor1TargetVelocity = target;
|
||||||
@@ -75,6 +141,8 @@ public class ShooterReal implements ShooterIO {
|
|||||||
// Math.abs(currentLimit.in(Amps)) > current &&
|
// Math.abs(currentLimit.in(Amps)) > current &&
|
||||||
// Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
|
// Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
|
||||||
// ) {
|
// ) {
|
||||||
|
state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput);
|
||||||
|
state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput);
|
||||||
m_shooter1Motor.set(percentOutput);
|
m_shooter1Motor.set(percentOutput);
|
||||||
m_shooter2Motor.set(percentOutput);
|
m_shooter2Motor.set(percentOutput);
|
||||||
// } else {
|
// } else {
|
||||||
|
|||||||
@@ -0,0 +1,204 @@
|
|||||||
|
package frc4388.robot.subsystems.swerve;
|
||||||
|
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
|
import edu.wpi.first.math.geometry.Translation2d;
|
||||||
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||||
|
import edu.wpi.first.wpilibj.DriverStation;
|
||||||
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
|
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||||
|
|
||||||
|
public class SimpleSwerveSim implements SwerveIO {
|
||||||
|
private Pose2d pose = new Pose2d();
|
||||||
|
private Pose2d lastPose = pose;
|
||||||
|
private double vx = 0.0;
|
||||||
|
private double vy = 0.0;
|
||||||
|
private double omega = 0.0;
|
||||||
|
|
||||||
|
private long lastTimeNs = System.nanoTime();
|
||||||
|
|
||||||
|
public SimpleSwerveSim() {
|
||||||
|
}
|
||||||
|
|
||||||
|
public synchronized void setControl(SwerveRequest ctrl) {
|
||||||
|
if (ctrl == null) return;
|
||||||
|
|
||||||
|
if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) {
|
||||||
|
vx = facingAngle.VelocityX;
|
||||||
|
vy = facingAngle.VelocityY;
|
||||||
|
|
||||||
|
double currentAngle = pose.getRotation().getRadians();
|
||||||
|
double targetAngle = facingAngle.TargetDirection.getRadians();
|
||||||
|
double error = targetAngle - currentAngle;
|
||||||
|
|
||||||
|
error = Math.atan2(Math.sin(error), Math.cos(error));
|
||||||
|
|
||||||
|
double kP = 5.0;
|
||||||
|
omega = error * kP;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ctrl instanceof SwerveRequest.FieldCentric fc) {
|
||||||
|
vx = fc.VelocityX;
|
||||||
|
vy = fc.VelocityY;
|
||||||
|
omega = fc.RotationalRate;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ctrl instanceof SwerveRequest.RobotCentric rc) {
|
||||||
|
double cos = pose.getRotation().getCos();
|
||||||
|
double sin = pose.getRotation().getSin();
|
||||||
|
double vxRobot = rc.VelocityX;
|
||||||
|
double vyRobot = rc.VelocityY;
|
||||||
|
vx = vxRobot * cos - vyRobot * sin;
|
||||||
|
vy = vxRobot * sin + vyRobot * cos;
|
||||||
|
omega = rc.RotationalRate;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ctrl instanceof SwerveRequest.SwerveDriveBrake) {
|
||||||
|
vx = 0; vy = 0; omega = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
ChassisSpeeds cs = tryGetSpeedsField(ctrl);
|
||||||
|
if (cs != null) {
|
||||||
|
vx = cs.vxMetersPerSecond;
|
||||||
|
vy = cs.vyMetersPerSecond;
|
||||||
|
omega = cs.omegaRadiansPerSecond;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) {
|
||||||
|
try {
|
||||||
|
java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds");
|
||||||
|
f.setAccessible(true);
|
||||||
|
Object o = f.get(ctrl);
|
||||||
|
if (o instanceof ChassisSpeeds) {
|
||||||
|
return (ChassisSpeeds) o;
|
||||||
|
}
|
||||||
|
} catch (NoSuchFieldException nsf) {
|
||||||
|
} catch (IllegalAccessException iae) {
|
||||||
|
} catch (SecurityException se) {
|
||||||
|
}
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
|
||||||
|
private double tryGetDoubleField(Object obj, Class<?> cls, String... names) {
|
||||||
|
for (String n : names) {
|
||||||
|
try {
|
||||||
|
java.lang.reflect.Field f = cls.getDeclaredField(n);
|
||||||
|
f.setAccessible(true);
|
||||||
|
Object val = f.get(obj);
|
||||||
|
if (val instanceof Number) {
|
||||||
|
return ((Number) val).doubleValue();
|
||||||
|
}
|
||||||
|
} catch (NoSuchFieldException nsf) {
|
||||||
|
} catch (IllegalAccessException iae) {
|
||||||
|
} catch (Throwable t) {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public synchronized void pointAtXY(double x, double y) {
|
||||||
|
Translation2d target = new Translation2d(x, y);
|
||||||
|
Translation2d toTarget = target.minus(pose.getTranslation());
|
||||||
|
if (toTarget.getNorm() < 1e-9) return;
|
||||||
|
Rotation2d desired = toTarget.getAngle();
|
||||||
|
pose = new Pose2d(pose.getTranslation(), desired);
|
||||||
|
lastPose = pose;
|
||||||
|
vx = 0.0;
|
||||||
|
vy = 0.0;
|
||||||
|
omega = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public synchronized void updateInputs(SwerveState state) {
|
||||||
|
if (state == null) return;
|
||||||
|
|
||||||
|
long now = System.nanoTime();
|
||||||
|
double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9);
|
||||||
|
lastTimeNs = now;
|
||||||
|
|
||||||
|
lastPose = pose;
|
||||||
|
double dxField;
|
||||||
|
double dyField;
|
||||||
|
|
||||||
|
if (DriverStation.isAutonomous()) {
|
||||||
|
double dxRobot = vx * dt;
|
||||||
|
double dyRobot = vy * dt;
|
||||||
|
|
||||||
|
Rotation2d r = pose.getRotation();
|
||||||
|
double cos = r.getCos();
|
||||||
|
double sin = r.getSin();
|
||||||
|
|
||||||
|
dxField = dxRobot * cos - dyRobot * sin;
|
||||||
|
dyField = dxRobot * sin + dyRobot * cos;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
dxField = vx * dt;
|
||||||
|
dyField = vy * dt;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
double dTheta = omega * dt;
|
||||||
|
|
||||||
|
Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField));
|
||||||
|
Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta));
|
||||||
|
pose = new Pose2d(newTrans, newRot);
|
||||||
|
|
||||||
|
state.lastPose = lastPose;
|
||||||
|
state.currentPose = pose;
|
||||||
|
state.speeds = new ChassisSpeeds(vx, vy, omega);
|
||||||
|
state.odometryRate = dt;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public synchronized void resetPose(Pose2d p) {
|
||||||
|
if (p == null) return;
|
||||||
|
pose = p;
|
||||||
|
lastPose = p;
|
||||||
|
lastTimeNs = System.nanoTime();
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public synchronized void tareEverything() {
|
||||||
|
pose = new Pose2d();
|
||||||
|
lastPose = pose;
|
||||||
|
vx = 0.0;
|
||||||
|
vy = 0.0;
|
||||||
|
omega = 0.0;
|
||||||
|
lastTimeNs = System.nanoTime();
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public synchronized void addVisionMeasurement(List<PoseObservation> poses) {
|
||||||
|
if (poses == null || poses.isEmpty()) return;
|
||||||
|
Pose2d visPose = poses.get(poses.size() - 1).pose();
|
||||||
|
if (visPose != null) {
|
||||||
|
pose = visPose;
|
||||||
|
lastPose = visPose;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public synchronized void pointAt(Translation2d target) {
|
||||||
|
if (target == null) return;
|
||||||
|
Translation2d toTarget = target.minus(pose.getTranslation());
|
||||||
|
if (toTarget.getNorm() < 1e-9) return;
|
||||||
|
Rotation2d desired = toTarget.getAngle();
|
||||||
|
pose = new Pose2d(pose.getTranslation(), desired);
|
||||||
|
lastPose = pose;
|
||||||
|
omega = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public synchronized void setLimits(double limitInAmps) {
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -5,7 +5,9 @@
|
|||||||
package frc4388.robot.subsystems.swerve;
|
package frc4388.robot.subsystems.swerve;
|
||||||
|
|
||||||
import static edu.wpi.first.units.Units.Rotations;
|
import static edu.wpi.first.units.Units.Rotations;
|
||||||
|
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||||
|
|
||||||
|
import java.util.Optional;
|
||||||
import java.util.function.Supplier;
|
import java.util.function.Supplier;
|
||||||
|
|
||||||
import org.littletonrobotics.junction.AutoLogOutput;
|
import org.littletonrobotics.junction.AutoLogOutput;
|
||||||
@@ -19,10 +21,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
|||||||
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
|
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
|
||||||
import com.pathplanner.lib.util.PathPlannerLogging;
|
import com.pathplanner.lib.util.PathPlannerLogging;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.controller.PIDController;
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
|
import edu.wpi.first.math.geometry.Transform2d;
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
import edu.wpi.first.math.geometry.Translation2d;
|
||||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||||
|
import edu.wpi.first.units.measure.AngularVelocity;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.Command;
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
@@ -63,6 +68,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
public Rotation2d orientRotTarget = new Rotation2d();
|
public Rotation2d orientRotTarget = new Rotation2d();
|
||||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||||
|
|
||||||
|
private final PIDController m_rotationOverridePID = new PIDController(
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kD.get()
|
||||||
|
);
|
||||||
|
private boolean m_useRotationOverride = false;
|
||||||
|
private Translation2d m_rotationOverrideTarget = new Translation2d();
|
||||||
|
|
||||||
/** Creates a new SwerveDrive. */
|
/** Creates a new SwerveDrive. */
|
||||||
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
|
public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
|
||||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
|
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
|
||||||
@@ -81,6 +94,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
// Handle exception as needed
|
// Handle exception as needed
|
||||||
config = null;
|
config = null;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
PPHolonomicDriveController driveController = new PPHolonomicDriveController(
|
||||||
|
new PIDConstants(5.0, 0.0, 0.0), // Translation PID
|
||||||
|
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF)
|
||||||
|
);
|
||||||
|
driveController.setRotationTargetOverride(() -> {
|
||||||
|
if (!m_useRotationOverride) return Optional.empty();
|
||||||
|
Rotation2d targetAngle = getPose2d()
|
||||||
|
.getTranslation()
|
||||||
|
.minus(m_rotationOverrideTarget)
|
||||||
|
.getAngle();
|
||||||
|
return Optional.of(targetAngle);
|
||||||
|
});
|
||||||
|
|
||||||
// DoubleSupplier a = () -> 1.d;
|
// DoubleSupplier a = () -> 1.d;
|
||||||
AutoBuilder.configure(
|
AutoBuilder.configure(
|
||||||
() -> {
|
() -> {
|
||||||
@@ -92,11 +119,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
|
(speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
|
||||||
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
|
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
|
||||||
// Also optionally outputs individual module feedforwards
|
// Also optionally outputs individual module feedforwards
|
||||||
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
|
driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...)
|
||||||
// holonomic drive trains
|
|
||||||
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
|
|
||||||
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
|
|
||||||
),
|
|
||||||
config, // The robot configuration
|
config, // The robot configuration
|
||||||
() -> {
|
() -> {
|
||||||
// Boolean supplier that controls when the path will be mirrored for the red
|
// Boolean supplier that controls when the path will be mirrored for the red
|
||||||
@@ -148,14 +171,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
public void setInitalPose(Pose2d startingAutoPose){
|
public void setInitalPose(Pose2d startingAutoPose){
|
||||||
initalPose2d = startingAutoPose;
|
initalPose2d = startingAutoPose;
|
||||||
}
|
}
|
||||||
// MIRA public void setOdoPose(Pose2d pose) {
|
|
||||||
// if (pose == null) return;
|
|
||||||
// io.tareEverything();
|
|
||||||
// initalPose2d = pose;
|
|
||||||
// io.resetPose(pose);
|
|
||||||
// robotKnowsWhereItIs = true;
|
|
||||||
// rotTarget = pose.getRotation().getDegrees();
|
|
||||||
// }
|
|
||||||
|
|
||||||
|
|
||||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
|
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
|
||||||
@@ -172,6 +187,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
// module.setDesiredState(state);
|
// module.setDesiredState(state);
|
||||||
// }
|
// }
|
||||||
|
|
||||||
|
public double chassisXSpeeds(){
|
||||||
|
if (TimesNegativeOne.isRed) {
|
||||||
|
return chassisSpeeds.vxMetersPerSecond;
|
||||||
|
} else {
|
||||||
|
return -chassisSpeeds.vxMetersPerSecond;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||||
stopModules(); // stop the swerve
|
stopModules(); // stop the swerve
|
||||||
@@ -234,6 +257,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) {
|
||||||
|
Translation2d robotSpeed = new Translation2d(
|
||||||
|
chassisSpeeds.vxMetersPerSecond,
|
||||||
|
chassisSpeeds.vyMetersPerSecond
|
||||||
|
);
|
||||||
|
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
|
||||||
|
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
|
||||||
|
|
||||||
|
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||||
|
.withVelocityX(chassisSpeeds.vxMetersPerSecond)
|
||||||
|
.withVelocityY(chassisSpeeds.vyMetersPerSecond)
|
||||||
|
.withTargetDirection(ang);
|
||||||
|
ctrl.HeadingController.setPID(
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kD.get()
|
||||||
|
);
|
||||||
|
io.setControl(ctrl);
|
||||||
|
}
|
||||||
|
|
||||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
|
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
|
||||||
// reason to have a robot
|
// reason to have a robot
|
||||||
// relitive version of
|
// relitive version of
|
||||||
@@ -301,7 +345,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
// SmartDashboard.putBoolean("drift correction", true);
|
// SmartDashboard.putBoolean("drift correction", true);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) {
|
||||||
|
|
||||||
|
rotTarget = heading.getDegrees();
|
||||||
|
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||||
|
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||||
|
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||||
|
.withTargetDirection(heading);
|
||||||
|
ctrl.HeadingController.setPID(
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
|
||||||
|
SwerveDriveConstants.PIDConstants.AIM_kD.get()
|
||||||
|
);
|
||||||
|
io.setControl(ctrl);
|
||||||
|
}
|
||||||
|
|
||||||
public void driveIntake(Translation2d leftStick, boolean invertRotation){
|
public void driveIntake(Translation2d leftStick, boolean invertRotation){
|
||||||
// if (invert){
|
// if (invert){
|
||||||
@@ -360,23 +417,57 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
|
|
||||||
// Drive with the robot facing towards a specific position
|
// Drive with the robot facing towards a specific position
|
||||||
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
|
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
|
||||||
|
Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
|
||||||
// Get the current speed of the robot
|
double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY());
|
||||||
Translation2d robotSpeed = new Translation2d(
|
if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){
|
||||||
chassisSpeeds.vxMetersPerSecond,
|
if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) {
|
||||||
chassisSpeeds.vyMetersPerSecond
|
if (TimesNegativeOne.isRed){
|
||||||
);
|
robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond);
|
||||||
|
} else {
|
||||||
// Calculate a point to aim ahead of the actual position.
|
robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond);
|
||||||
Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
|
}
|
||||||
|
} }
|
||||||
// Calculate the angle between the current position and the lead position
|
Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
|
||||||
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
|
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
|
||||||
|
Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d());
|
||||||
|
Logger.recordOutput("Lead Aim", fieldPosLeadLog);
|
||||||
|
driveFieldAngle(leftStick, ang);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void offsetOdoPosition(Transform2d offset) {
|
||||||
|
// Manually performing an addittion on the pose
|
||||||
|
// WHY doesn't WPILIB have the ability to not transform poses
|
||||||
|
Pose2d new_pose = new Pose2d(
|
||||||
|
new Translation2d(
|
||||||
|
state.currentPose.getX() + offset.getX(),
|
||||||
|
state.currentPose.getY() + offset.getY()
|
||||||
|
),
|
||||||
|
state.currentPose.getRotation()
|
||||||
|
);
|
||||||
|
this.io.resetPose(new_pose);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void defenseXPosition(){
|
||||||
|
io.setControl(new SwerveRequest.SwerveDriveBrake());
|
||||||
|
}
|
||||||
|
|
||||||
|
public void stopDefenseXPosition(){
|
||||||
|
stopModules();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
|
||||||
|
// Calculate the angle between the current position and the lead position
|
||||||
|
//Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle();
|
||||||
|
Rotation2d ang = new Rotation2d(0,1);
|
||||||
|
System.out.println(ang);
|
||||||
|
|
||||||
driveFieldAngle(leftStick, ang);
|
driveFieldAngle(leftStick, ang);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public Pose2d getCurrentPose(){
|
||||||
|
return state.currentPose;
|
||||||
|
}
|
||||||
|
|
||||||
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
|
||||||
leftStick = leftStick.rotateBy(heading);
|
leftStick = leftStick.rotateBy(heading);
|
||||||
|
|
||||||
@@ -464,6 +555,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
softStop();
|
softStop();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) {
|
||||||
|
Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
|
||||||
|
|
||||||
|
Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
|
||||||
|
m_rotationOverrideTarget = fieldPosLead;
|
||||||
|
m_useRotationOverride = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void disableRotationOverride() {
|
||||||
|
m_useRotationOverride = false;
|
||||||
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void periodic() {
|
public void periodic() {
|
||||||
// This method will be called once per scheduler run\
|
// This method will be called once per scheduler run\
|
||||||
@@ -476,6 +579,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
vision.setLastOdomPose(state.currentPose);
|
vision.setLastOdomPose(state.currentPose);
|
||||||
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||||
|
|
||||||
|
if (state.speeds != null) {
|
||||||
|
this.chassisSpeeds = state.speeds;
|
||||||
|
} else {
|
||||||
|
this.chassisSpeeds = new ChassisSpeeds();
|
||||||
|
}
|
||||||
|
|
||||||
if (vision.isTag()) {
|
if (vision.isTag()) {
|
||||||
Pose2d pose = vision.getPose2d();
|
Pose2d pose = vision.getPose2d();
|
||||||
if (!robotKnowsWhereItIs) {
|
if (!robotKnowsWhereItIs) {
|
||||||
@@ -485,6 +594,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
io.addVisionMeasurement(vision.getPosesToAdd());
|
io.addVisionMeasurement(vision.getPosesToAdd());
|
||||||
|
io.updateInputs(state);
|
||||||
|
Logger.processInputs("SwerveDrive", state);
|
||||||
|
|
||||||
|
vision.setLastOdomPose(state.currentPose);
|
||||||
|
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
|
||||||
}
|
}
|
||||||
|
|
||||||
// if(e.isPresent())
|
// if(e.isPresent())
|
||||||
@@ -515,6 +629,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
gear_index = i;
|
gear_index = i;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
public void setPercentOutput(double speed) {
|
public void setPercentOutput(double speed) {
|
||||||
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||||
gear_index = -1;
|
gear_index = -1;
|
||||||
@@ -590,4 +705,3 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
|||||||
return status;
|
return status;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -73,9 +73,10 @@ public final class SwerveDriveConstants {
|
|||||||
public static final boolean INVERT_Y = true;
|
public static final boolean INVERT_Y = true;
|
||||||
public static final boolean INVERT_ROTATION = false;
|
public static final boolean INVERT_ROTATION = false;
|
||||||
|
|
||||||
|
public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05);
|
||||||
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
|
||||||
|
|
||||||
private static final class ModuleSpecificConstants { //2025
|
public static final class ModuleSpecificConstants { //2026
|
||||||
//Front Left
|
//Front Left
|
||||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
|
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
|
||||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||||
|
|||||||
@@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog;
|
|||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||||
|
import frc4388.utility.status.CanDevice;
|
||||||
|
|
||||||
public interface SwerveIO {
|
public interface SwerveIO {
|
||||||
@AutoLog
|
@AutoLog
|
||||||
|
|||||||
@@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule;
|
|||||||
import com.ctre.phoenix6.swerve.SwerveRequest;
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Pose2d;
|
import edu.wpi.first.math.geometry.Pose2d;
|
||||||
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import frc4388.robot.subsystems.vision.Vision;
|
import frc4388.robot.subsystems.vision.Vision;
|
||||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||||
|
import frc4388.utility.status.CanDevice;
|
||||||
|
|
||||||
public class SwerveReal implements SwerveIO {
|
public class SwerveReal implements SwerveIO {
|
||||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||||
|
|||||||
+3
-3
@@ -1,4 +1,4 @@
|
|||||||
package frc4388.robot.subsystems;
|
package frc4388.robot.subsystems.vision;
|
||||||
|
|
||||||
import java.util.ArrayList;
|
import java.util.ArrayList;
|
||||||
import java.util.LinkedList;
|
import java.util.LinkedList;
|
||||||
@@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d;
|
|||||||
import edu.wpi.first.math.util.Units;
|
import edu.wpi.first.math.util.Units;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
import frc4388.robot.subsystems.RPLidarA1.PolarPoint;
|
import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint;
|
||||||
import frc4388.robot.subsystems.RPLidarA1.ScanListener;
|
import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener;
|
||||||
import frc4388.utility.configurable.ConfigurableDouble;
|
import frc4388.utility.configurable.ConfigurableDouble;
|
||||||
import frc4388.utility.status.FaultA1M8;
|
import frc4388.utility.status.FaultA1M8;
|
||||||
|
|
||||||
+1
-1
@@ -1,4 +1,4 @@
|
|||||||
package frc4388.robot.subsystems;
|
package frc4388.robot.subsystems.vision;
|
||||||
|
|
||||||
import com.fazecast.jSerialComm.SerialPort;
|
import com.fazecast.jSerialComm.SerialPort;
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
import edu.wpi.first.wpilibj.DriverStation;
|
||||||
@@ -0,0 +1,154 @@
|
|||||||
|
|
||||||
|
package frc4388.utility.compute;
|
||||||
|
import edu.wpi.first.wpilibj.DriverStation;
|
||||||
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||||
|
import edu.wpi.first.wpilibj.Timer;
|
||||||
|
public class HubShiftTimer {
|
||||||
|
|
||||||
|
public enum ShiftPhase {
|
||||||
|
DISABLED,
|
||||||
|
AUTO,
|
||||||
|
TRANSITION, // 0 – 10 s
|
||||||
|
SHIFT1, // 10 – 35 s
|
||||||
|
SHIFT2, // 35 – 60 s
|
||||||
|
SHIFT3, // 60 – 85 s
|
||||||
|
SHIFT4, // 85 – 110 s
|
||||||
|
ENDGAME // 110 – 140 s
|
||||||
|
}
|
||||||
|
|
||||||
|
public record ShiftInfo(
|
||||||
|
ShiftPhase phase,
|
||||||
|
double elapsedInShift,
|
||||||
|
double remainingInShift,
|
||||||
|
boolean isActive) {}
|
||||||
|
//total teleop time
|
||||||
|
public static final double TELEOP_DURATION = 140.0;
|
||||||
|
//total auto time
|
||||||
|
public static final double AUTO_DURATION = 20.0;
|
||||||
|
|
||||||
|
//shift start and end times for calculations
|
||||||
|
private static final double[] SHIFT_STARTS = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0};
|
||||||
|
private static final double[] SHIFT_ENDS = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0};
|
||||||
|
|
||||||
|
//hub active schedule, true is active and false is inactive
|
||||||
|
//starts always as active becasue transition is first and is active, but then is inactive for winner or active for loser
|
||||||
|
private static final boolean[] WINNER_SCHEDULE = {true, false, true, false, true, true};
|
||||||
|
private static final boolean[] LOSER_SCHEDULE = {true, true, false, true, false, true};
|
||||||
|
|
||||||
|
//shift phase names during teleop
|
||||||
|
private static final ShiftPhase[] SHIFT_PHASES = {
|
||||||
|
ShiftPhase.TRANSITION,
|
||||||
|
ShiftPhase.SHIFT1,
|
||||||
|
ShiftPhase.SHIFT2,
|
||||||
|
ShiftPhase.SHIFT3,
|
||||||
|
ShiftPhase.SHIFT4,
|
||||||
|
ShiftPhase.ENDGAME
|
||||||
|
};
|
||||||
|
|
||||||
|
//timer to track time
|
||||||
|
private static final Timer teleopTimer = new Timer();
|
||||||
|
private static double timerOffset = 0.0;
|
||||||
|
|
||||||
|
//fms syncing idk other team did it too
|
||||||
|
private static final double RESYNC_THRESHOLD = 3.0;
|
||||||
|
|
||||||
|
|
||||||
|
//call at start of auto to start timer
|
||||||
|
public static void initializeAuto() {
|
||||||
|
teleopTimer.restart();
|
||||||
|
}
|
||||||
|
|
||||||
|
//call at start of teleop to start timer again because sometimes delay between auto and telop
|
||||||
|
public static void initializeTeleop() {
|
||||||
|
timerOffset = 0.0;
|
||||||
|
teleopTimer.restart();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//returns the updated shift info based on the winner of auto
|
||||||
|
public static ShiftInfo getShiftInfo() {
|
||||||
|
if (!DriverStation.isEnabled()) {
|
||||||
|
return new ShiftInfo(ShiftPhase.DISABLED, 0.0, 0.0, false);
|
||||||
|
}
|
||||||
|
if (DriverStation.isAutonomousEnabled()) {
|
||||||
|
double autoElapsed = teleopTimer.get(); // timer restarts in initialize()
|
||||||
|
return new ShiftInfo(
|
||||||
|
ShiftPhase.AUTO,
|
||||||
|
autoElapsed,
|
||||||
|
Math.max(0.0, AUTO_DURATION - teleopTimer.get()),
|
||||||
|
true);
|
||||||
|
}
|
||||||
|
return computeTeleopShift();
|
||||||
|
}
|
||||||
|
|
||||||
|
//find auto winner, R = red wins, B = blue wins
|
||||||
|
public static Alliance autoWinnerAlliance() {
|
||||||
|
String msg = DriverStation.getGameSpecificMessage();
|
||||||
|
if (msg != null && msg.length() > 0) {
|
||||||
|
char c = msg.charAt(0);
|
||||||
|
if (c == 'R') return Alliance.Red;
|
||||||
|
if (c == 'B') return Alliance.Blue;
|
||||||
|
}
|
||||||
|
// backup if no msg, returns auto winner as opposite of our alliance. if we red -> blue wins auto
|
||||||
|
Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue);
|
||||||
|
return (ours == Alliance.Blue) ? Alliance.Red : Alliance.Blue;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//return our schedule for the shifts
|
||||||
|
private static boolean[] getSchedule() {
|
||||||
|
Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue);
|
||||||
|
Alliance winner = autoWinnerAlliance();
|
||||||
|
return (ours == winner) ? WINNER_SCHEDULE : LOSER_SCHEDULE;
|
||||||
|
}
|
||||||
|
|
||||||
|
//time since start of teleop
|
||||||
|
private static double getTeleopElapsed() {
|
||||||
|
double localTime = teleopTimer.get() - timerOffset;
|
||||||
|
|
||||||
|
// Re-sync to FMS time if we drift too far (only when FMS is attached)
|
||||||
|
if (DriverStation.isFMSAttached()) {
|
||||||
|
double fmsElapsed = TELEOP_DURATION - DriverStation.getMatchTime();
|
||||||
|
if (fmsElapsed <= TELEOP_DURATION - 5.0 // ignore the first few seconds of jitter
|
||||||
|
&& Math.abs(localTime - fmsElapsed) >= RESYNC_THRESHOLD) {
|
||||||
|
timerOffset += localTime - fmsElapsed;
|
||||||
|
localTime = fmsElapsed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return Math.max(0.0, Math.min(TELEOP_DURATION, localTime));
|
||||||
|
}
|
||||||
|
|
||||||
|
private static ShiftInfo computeTeleopShift() {
|
||||||
|
boolean[] schedule = getSchedule();
|
||||||
|
double elapsed = getTeleopElapsed();
|
||||||
|
|
||||||
|
// Find which shift we're in
|
||||||
|
int phaseIndex = SHIFT_STARTS.length - 1; // default to last shift if past all bounds
|
||||||
|
for (int i = 0; i < SHIFT_STARTS.length; i++) {
|
||||||
|
if (elapsed >= SHIFT_STARTS[i] && elapsed < SHIFT_ENDS[i]) {
|
||||||
|
phaseIndex = i;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex];
|
||||||
|
double shiftRemaining = SHIFT_ENDS[phaseIndex] - elapsed;
|
||||||
|
|
||||||
|
// merge time for elapsed if same active/inactive
|
||||||
|
if (phaseIndex > 0 && schedule[phaseIndex] == schedule[phaseIndex - 1]) {
|
||||||
|
shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex - 1];
|
||||||
|
}
|
||||||
|
|
||||||
|
// merge time for remaining time if same active/inactive status
|
||||||
|
if (phaseIndex < SHIFT_ENDS.length - 1 && schedule[phaseIndex] == schedule[phaseIndex + 1]) {
|
||||||
|
shiftRemaining = SHIFT_ENDS[phaseIndex + 1] - elapsed;
|
||||||
|
}
|
||||||
|
|
||||||
|
return new ShiftInfo(
|
||||||
|
SHIFT_PHASES[phaseIndex],
|
||||||
|
shiftElapsed,
|
||||||
|
shiftRemaining,
|
||||||
|
schedule[phaseIndex]);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -50,4 +50,9 @@ public class CanDevice {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public Object getId() {
|
||||||
|
return id;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
package frc4388.utility.status;
|
package frc4388.utility.status;
|
||||||
|
|
||||||
import frc4388.robot.subsystems.RPLidarA1;
|
import frc4388.robot.subsystems.vision.RPLidarA1;
|
||||||
import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus;
|
import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus;
|
||||||
import frc4388.utility.status.Status.ReportLevel;
|
import frc4388.utility.status.Status.ReportLevel;
|
||||||
|
|
||||||
// Fault reporter for the RPLidar A1M8 Revolving lidar sensor
|
// Fault reporter for the RPLidar A1M8 Revolving lidar sensor
|
||||||
|
|||||||
Reference in New Issue
Block a user