diff --git a/simgui.json b/simgui.json
index bc02272..d725fe9 100644
--- a/simgui.json
+++ b/simgui.json
@@ -24,12 +24,7 @@
"visible": true
}
},
- "/SmartDashboard/Arm Mechanism": {
- "window": {
- "visible": true
- }
- },
- "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": {
+ "/SmartDashboard/SendableChooser[0]": {
"window": {
"visible": true
}
@@ -43,35 +38,29 @@
"transitory": {
"Shuffleboard": {
"Arm": {
- "Derivatives": {
- "open": true
- },
"Feedforward": {
"open": true
},
- "Goal": {
- "open": true
- },
"Position": {
"open": true
},
- "Setpoint": {
- "open": true
- },
"Velocities": {
"open": true
},
- "Voltages": {
- "open": true
- },
"open": true
},
"Climber": {
+ "East": {
+ "open": true
+ },
"Elevator": {
"open": true
},
"Linear Actuator": {
"open": true
+ },
+ "West": {
+ "open": true
}
},
"Intake": {
diff --git a/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto b/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto
new file mode 100644
index 0000000..185323b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto
@@ -0,0 +1,37 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.8,
+ "y": 6.63
+ },
+ "rotation": 60.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "stow"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/amp.full.auto b/src/main/deploy/pathplanner/autos/amp.full.auto
new file mode 100644
index 0000000..d579d60
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/amp.full.auto
@@ -0,0 +1,55 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.8,
+ "y": 6.63
+ },
+ "rotation": 60.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "amp.toNearest"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "amp.toSubwoofer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": null
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/amp.home.auto b/src/main/deploy/pathplanner/autos/amp.home.auto
new file mode 100644
index 0000000..d903830
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/amp.home.auto
@@ -0,0 +1,25 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.8,
+ "y": 6.63
+ },
+ "rotation": 60.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/amp.auto b/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto
similarity index 78%
rename from src/main/deploy/pathplanner/autos/amp.auto
rename to src/main/deploy/pathplanner/autos/amp.leaveOnly.auto
index 5695d2b..295a313 100644
--- a/src/main/deploy/pathplanner/autos/amp.auto
+++ b/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto
@@ -12,15 +12,15 @@
"data": {
"commands": [
{
- "type": "path",
+ "type": "named",
"data": {
- "pathName": "amp.toNearest"
+ "name": "home"
}
},
{
"type": "path",
"data": {
- "pathName": "amp.toSubwoofer"
+ "pathName": "amp.leave"
}
}
]
diff --git a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto
new file mode 100644
index 0000000..8572861
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto
@@ -0,0 +1,43 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.8,
+ "y": 6.63
+ },
+ "rotation": 60.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "stow"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "amp.leave"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/center.full.auto b/src/main/deploy/pathplanner/autos/center.full.auto
new file mode 100644
index 0000000..836de3c
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/center.full.auto
@@ -0,0 +1,61 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 1.36,
+ "y": 5.56
+ },
+ "rotation": 0.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "readyShoot"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "center.toNearest"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "intakeNote"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "center.toSubwoofer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/center.home.auto b/src/main/deploy/pathplanner/autos/center.home.auto
new file mode 100644
index 0000000..aeb8e4b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/center.home.auto
@@ -0,0 +1,25 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 1.36,
+ "y": 5.56
+ },
+ "rotation": 0.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/center.leaveOnly.auto b/src/main/deploy/pathplanner/autos/center.leaveOnly.auto
new file mode 100644
index 0000000..a4a6808
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/center.leaveOnly.auto
@@ -0,0 +1,31 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 1.36,
+ "y": 5.56
+ },
+ "rotation": 0.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "center.leave"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/source.auto b/src/main/deploy/pathplanner/autos/source.auto
new file mode 100644
index 0000000..9cc6469
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/source.auto
@@ -0,0 +1,31 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.62,
+ "y": 2.27
+ },
+ "rotation": 0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "source.backout"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/stage.full.auto b/src/main/deploy/pathplanner/autos/stage.full.auto
new file mode 100644
index 0000000..57476a3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/stage.full.auto
@@ -0,0 +1,74 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.73,
+ "y": 4.39
+ },
+ "rotation": -60.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "readyShoot"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "stage.toNearest"
+ }
+ },
+ {
+ "type": "race",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "intakeNote"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ }
+ ]
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "stage.toSubwoofer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "shootNote"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/stage.home.auto b/src/main/deploy/pathplanner/autos/stage.home.auto
new file mode 100644
index 0000000..254117d
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/stage.home.auto
@@ -0,0 +1,25 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.8108477755417065,
+ "y": 4.311854008323402
+ },
+ "rotation": -57.20046872738075
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto b/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto
new file mode 100644
index 0000000..3148f23
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto
@@ -0,0 +1,31 @@
+{
+ "version": 1.0,
+ "startingPose": {
+ "position": {
+ "x": 0.73,
+ "y": 4.39
+ },
+ "rotation": -60.0
+ },
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "home"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "stage.leave"
+ }
+ }
+ ]
+ }
+ },
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/amp.leave.path b/src/main/deploy/pathplanner/paths/amp.leave.path
new file mode 100644
index 0000000..877aa20
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/amp.leave.path
@@ -0,0 +1,52 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 0.8009808327308976,
+ "y": 6.630585568863446
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 1.0648639126144392,
+ "y": 7.087644470479496
+ },
+ "isLocked": false,
+ "linkedName": "amp.subwoofer"
+ },
+ {
+ "anchor": {
+ "x": 5.448310896621795,
+ "y": 7.114065766593072
+ },
+ "prevControl": {
+ "x": 5.042684729899664,
+ "y": 7.114065766593072
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": 60.0,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/amp.toNearest.path b/src/main/deploy/pathplanner/paths/amp.toNearest.path
index 3ef3105..5956295 100644
--- a/src/main/deploy/pathplanner/paths/amp.toNearest.path
+++ b/src/main/deploy/pathplanner/paths/amp.toNearest.path
@@ -16,15 +16,15 @@
},
{
"anchor": {
- "x": 2.3,
- "y": 7.0
+ "x": 3.188780992946603,
+ "y": 7.025263281295794
},
"prevControl": {
- "x": 1.8943738332778692,
- "y": 7.0
+ "x": 2.7831548262244725,
+ "y": 7.025263281295794
},
"nextControl": null,
- "isLocked": true,
+ "isLocked": false,
"linkedName": "amp.nearestNote"
}
],
@@ -58,6 +58,23 @@
]
}
}
+ },
+ {
+ "name": "New Event Marker",
+ "waypointRelativePos": 0.5,
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "intakeNote"
+ }
+ }
+ ]
+ }
+ }
}
],
"globalConstraints": {
diff --git a/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path b/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path
index ce24fcc..d1f8562 100644
--- a/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path
+++ b/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
- "x": 2.3,
- "y": 7.0
+ "x": 3.188780992946603,
+ "y": 7.025263281295794
},
"prevControl": null,
"nextControl": {
- "x": 1.7778081710009588,
- "y": 7.114065766593072
+ "x": 2.6665891639475623,
+ "y": 7.139329047888865
},
"isLocked": false,
"linkedName": "amp.nearestNote"
@@ -39,7 +39,7 @@
"eventMarkers": [
{
"name": "New Event Marker",
- "waypointRelativePos": 0.1,
+ "waypointRelativePos": 0.2,
"command": {
"type": "sequential",
"data": {
diff --git a/src/main/deploy/pathplanner/paths/center.leave.path b/src/main/deploy/pathplanner/paths/center.leave.path
new file mode 100644
index 0000000..f4e336e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/center.leave.path
@@ -0,0 +1,52 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 1.3633965729469937,
+ "y": 5.555088802485298
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.3633965729469937,
+ "y": 5.555088802485298
+ },
+ "isLocked": false,
+ "linkedName": "center.subwoofer"
+ },
+ {
+ "anchor": {
+ "x": 5.4285770110001765,
+ "y": 6.640452511674254
+ },
+ "prevControl": {
+ "x": 4.4285770110001765,
+ "y": 6.640452511674254
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": 0,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/center.toNearest.path b/src/main/deploy/pathplanner/paths/center.toNearest.path
new file mode 100644
index 0000000..ccad971
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/center.toNearest.path
@@ -0,0 +1,70 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 1.3633965729469937,
+ "y": 5.555088802485298
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.3633965729469937,
+ "y": 5.555088802485298
+ },
+ "isLocked": false,
+ "linkedName": "center.subwoofer"
+ },
+ {
+ "anchor": {
+ "x": 2.409292510892716,
+ "y": 5.555088802485298
+ },
+ "prevControl": {
+ "x": 1.409292510892716,
+ "y": 5.555088802485298
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": "center.nearestNote"
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [
+ {
+ "name": "New Event Marker",
+ "waypointRelativePos": 0.8,
+ "command": {
+ "type": "race",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "intakeNote"
+ }
+ }
+ ]
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": 0,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/center.toSubwoofer.path b/src/main/deploy/pathplanner/paths/center.toSubwoofer.path
new file mode 100644
index 0000000..6a9d2c6
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/center.toSubwoofer.path
@@ -0,0 +1,52 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 2.409292510892716,
+ "y": 5.555088802485298
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 3.409292510892716,
+ "y": 5.555088802485298
+ },
+ "isLocked": false,
+ "linkedName": "center.nearestNote"
+ },
+ {
+ "anchor": {
+ "x": 1.3633965729469937,
+ "y": 5.555088802485298
+ },
+ "prevControl": {
+ "x": 1.7284734569469153,
+ "y": 5.584689630917723
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": "center.subwoofer"
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": 0,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/source.backout.path b/src/main/deploy/pathplanner/paths/source.backout.path
new file mode 100644
index 0000000..9f091a0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/source.backout.path
@@ -0,0 +1,52 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 0.62,
+ "y": 2.2719022898816124
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 1.6199999999999997,
+ "y": 2.2719022898816124
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.733583903687064,
+ "y": 1.475565404768469
+ },
+ "prevControl": {
+ "x": 3.2345968258270292,
+ "y": 1.6395171164094098
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": 0,
+ "velocity": 0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/stage.leave.path b/src/main/deploy/pathplanner/paths/stage.leave.path
new file mode 100644
index 0000000..0ec2c59
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/stage.leave.path
@@ -0,0 +1,52 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 0.7636103146671326,
+ "y": 4.403274541213847
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 0.8820136283968368,
+ "y": 4.2848712274841425
+ },
+ "isLocked": false,
+ "linkedName": "stage.subwoofer"
+ },
+ {
+ "anchor": {
+ "x": 5.408843125378559,
+ "y": 1.0261620523241057
+ },
+ "prevControl": {
+ "x": 4.408843125378559,
+ "y": 1.0261620523241057
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": -59.88626684901758,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/stage.toNearest.path b/src/main/deploy/pathplanner/paths/stage.toNearest.path
new file mode 100644
index 0000000..b7933bb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/stage.toNearest.path
@@ -0,0 +1,93 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 0.7636103146671326,
+ "y": 4.403274541213847
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 0.8820136283968368,
+ "y": 4.2848712274841425
+ },
+ "isLocked": false,
+ "linkedName": "stage.subwoofer"
+ },
+ {
+ "anchor": {
+ "x": 2.290889197163011,
+ "y": 4.124382094918037
+ },
+ "prevControl": {
+ "x": 1.2908891971630112,
+ "y": 4.124382094918037
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": "stage.nearestNote"
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [
+ {
+ "name": "New Event Marker",
+ "waypointRelativePos": 0.1,
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "readyIntake"
+ }
+ }
+ ]
+ }
+ }
+ },
+ {
+ "name": "New Event Marker",
+ "waypointRelativePos": 0.6,
+ "command": {
+ "type": "race",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "intakeNote"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.0
+ }
+ }
+ ]
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": -59.88626684901758,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path b/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path
new file mode 100644
index 0000000..bd29c30
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path
@@ -0,0 +1,70 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 2.290889197163011,
+ "y": 4.124382094918037
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 1.7356097479669974,
+ "y": 3.8879977331994615
+ },
+ "isLocked": false,
+ "linkedName": "stage.nearestNote"
+ },
+ {
+ "anchor": {
+ "x": 0.7636103146671326,
+ "y": 4.403274541213847
+ },
+ "prevControl": {
+ "x": 0.7636103146671326,
+ "y": 4.028527771748839
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": "stage.subwoofer"
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [
+ {
+ "name": "New Event Marker",
+ "waypointRelativePos": 0.1,
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "readyShoot"
+ }
+ }
+ ]
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -53.123077204898124,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": null,
+ "previewStartingState": {
+ "rotation": -1.0045856204165913,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 001a630..dee8f36 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,7 +5,6 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.arm.Arm;
import frc.robot.swerve.Swerve;
public class Robot extends TimedRobot {
@@ -19,7 +18,7 @@ public void robotInit() {
robotContainer = RobotContainer.getInstance();
swerve = Swerve.getInstance();
- new Trigger(this::isEnabled).whileTrue(Arm.getInstance().home());
+ // new Trigger(this::isEnabled).whileTrue(Arm.getInstance().home());
new Trigger(this::isDisabled)
.debounce(RobotConstants.DISABLE_COAST_DELAY)
diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java
index f217aeb..f11c6e3 100644
--- a/src/main/java/frc/robot/RobotConstants.java
+++ b/src/main/java/frc/robot/RobotConstants.java
@@ -44,7 +44,14 @@ public enum Subsystem {
SWERVE,
}
- public static final Set<Subsystem> REAL_SUBSYSTEMS =
+ public static final Set<Subsystem> ALL_SUBSYSTEMS =
EnumSet.of(
- Subsystem.ARM, Subsystem.INTAKE, Subsystem.ODOMETRY, Subsystem.SHOOTER, Subsystem.SWERVE);
+ Subsystem.ARM,
+ Subsystem.CLIMBER,
+ Subsystem.INTAKE,
+ Subsystem.ODOMETRY,
+ Subsystem.SHOOTER,
+ Subsystem.SWERVE);
+
+ public static final Set<Subsystem> REAL_SUBSYSTEMS = ALL_SUBSYSTEMS;
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5cc4c9e..9cf8d7a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -2,10 +2,12 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.lib.Telemetry;
import frc.robot.arm.Arm;
import frc.robot.auto.Auto;
+import frc.robot.climber.Climber;
import frc.robot.intake.Intake;
import frc.robot.odometry.Odometry;
import frc.robot.shooter.Shooter;
@@ -20,6 +22,7 @@ public class RobotContainer {
private final Arm arm;
private final Auto auto;
+ private final Climber climber;
private final Intake intake;
private final Odometry odometry;
private final Shooter shooter;
@@ -31,6 +34,7 @@ public class RobotContainer {
private RobotContainer() {
arm = Arm.getInstance();
auto = Auto.getInstance();
+ climber = Climber.getInstance();
intake = Intake.getInstance();
odometry = Odometry.getInstance();
shooter = Shooter.getInstance();
@@ -59,8 +63,7 @@ public static RobotContainer getInstance() {
/** Initializes subsystem telemetry. */
private void initializeTelemetry() {
if (RobotConstants.USE_TELEMETRY) {
- Telemetry.initializeShuffleboards(arm);
- SmartDashboard.putData("Arm Mechanism", RobotMechanisms.getInstance().getMechanism());
+ Telemetry.initializeShuffleboards(arm, climber, intake, shooter, swerve);
}
SmartDashboard.putData(auto.getAutonomousChooser());
@@ -90,6 +93,11 @@ private void configureBindings() {
.whileFalse(auto.stow());
operatorController.rightBumper().whileTrue(shooter.serialize());
+ operatorController.povUp().whileTrue(climber.up());
+ operatorController
+ .povDown()
+ .whileTrue(Commands.parallel(auto.readyIntake().repeatedly(), climber.down()));
+
operatorController.a().whileTrue(arm.amp()).whileFalse(auto.stow());
}
diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
index d297d80..2b729cc 100644
--- a/src/main/java/frc/robot/arm/Arm.java
+++ b/src/main/java/frc/robot/arm/Arm.java
@@ -7,7 +7,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
-import frc.robot.RobotMechanisms;
import frc.robot.arm.LimitSwitchIO.LimitSwitchIOValues;
import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
import frc.robot.arm.WristMotorIO.WristMotorIOValues;
@@ -48,7 +47,7 @@ private Arm() {
shoulderMotor.configure();
wristMotor.configure();
- ArmState initialState = ArmState.STOW.withShoulder(Rotation2d.fromDegrees(45));
+ ArmState initialState = ArmState.INIT;
setPosition(initialState);
@@ -77,17 +76,20 @@ public void periodic() {
shoulderMotor.update(shoulderMotorValues);
wristMotor.update(wristMotorValues);
- if (limitSwitchValues.isPressed) {
- setPosition(getPosition().withShoulder(ArmState.STOW.shoulder()));
- }
-
setSetpoint(setpoint.nextSetpoint(goal));
-
- RobotMechanisms.getInstance().setArmState(getPosition());
}
@Override
public void addToShuffleboard(ShuffleboardTab tab) {
+ ShuffleboardLayout commands = Telemetry.addColumn(tab, "Commands");
+
+ commands.addString(
+ "Running Command",
+ () ->
+ this.getCurrentCommand() != null
+ ? this.getCurrentCommand().getName()
+ : "no running command");
+
ShuffleboardLayout limitSwitch = Telemetry.addColumn(tab, "Limit Switch");
limitSwitch.addBoolean("Is Pressed?", () -> limitSwitchValues.isPressed);
@@ -189,7 +191,7 @@ private MoveShoulderCommand moveShoulder(ArmState goal) {
return new MoveShoulderCommand(goal);
}
- private MoveWristCommand moveWrist(ArmState goal) {
+ public MoveWristCommand moveWrist(ArmState goal) {
return new MoveWristCommand(goal);
}
@@ -213,6 +215,7 @@ public Command home() {
if (!interrupted) {
setPosition(ArmState.STOW);
}
- });
+ })
+ .withTimeout(3.0);
}
}
diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java
index 898cc7d..3b08ea5 100644
--- a/src/main/java/frc/robot/arm/ArmConstants.java
+++ b/src/main/java/frc/robot/arm/ArmConstants.java
@@ -88,10 +88,10 @@ public static class WristMotorConstants {
public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0);
/** Proportional gain in volts per rotation. */
- public static final double KP = 48.0;
+ public static final double KP = 36.0;
/** Maximum speed of the wrist joint in rotations per second. */
- public static final double MAXIMUM_SPEED = 2.4;
+ public static final double MAXIMUM_SPEED = 1.2;
/** Maximum acceleration of the wrist joint in rotations per second per second. */
public static final double MAXIMUM_ACCELERATION =
diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java
index c35d202..6e88117 100644
--- a/src/main/java/frc/robot/arm/ArmState.java
+++ b/src/main/java/frc/robot/arm/ArmState.java
@@ -16,8 +16,10 @@ public record ArmState(State shoulder, State wrist) {
public static final ArmState STOW =
new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE);
- public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(23.265));
- public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(6.81));
+
+ public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(18));
+
+ public static final ArmState INTAKE = STOW.withWrist(Rotation2d.fromDegrees(4));
public static final ArmState AMP =
new ArmState(ShoulderMotorConstants.MAXIMUM_ANGLE, Rotation2d.fromDegrees(0));
diff --git a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java
index f782063..4f9c1ec 100644
--- a/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java
+++ b/src/main/java/frc/robot/arm/WristMotorIOSparkMax.java
@@ -23,7 +23,8 @@ public class WristMotorIOSparkMax implements WristMotorIO {
private final AccelerationCalculator accelerationCalculator;
public WristMotorIOSparkMax() {
- sparkMax = new CANSparkMax(WristMotorConstants.CAN.id(), MotorType.kBrushless);
+ // TODO Temporary fix, using one of the climber Sparks
+ sparkMax = new CANSparkMax(6, MotorType.kBrushless);
feedback = new PIDController(WristMotorConstants.KP, 0, 0);
diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java
index e15b79d..c8c0057 100644
--- a/src/main/java/frc/robot/auto/Auto.java
+++ b/src/main/java/frc/robot/auto/Auto.java
@@ -82,6 +82,8 @@ private Auto() {
AllianceFlipHelper::shouldFlip,
swerve);
+ NamedCommands.registerCommand("home", Arm.getInstance().home());
+ NamedCommands.registerCommand("stow", stow());
NamedCommands.registerCommand("readyIntake", readyIntake());
NamedCommands.registerCommand("intakeNote", intakeNote());
NamedCommands.registerCommand("readyShoot", readyShoot());
@@ -130,25 +132,31 @@ public SendableChooser<Command> getAutonomousChooser() {
}
public Command readyIntake() {
+ double seconds = 3.0;
+
return Commands.parallel(
Commands.waitUntil(intake::isNotStowed).andThen(arm.moveShoulderThenWrist(ArmState.INTAKE)),
- intake.out());
+ intake.out()).withTimeout(seconds);
}
public Command intakeNote() {
- return Commands.parallel(intake.intake(), shooter.intake());
+ return readyIntake().andThen(Commands.parallel(intake.intake(), shooter.intake()));
}
public Command stow() {
+ double seconds = 2.0;
+
return Commands.parallel(
arm.moveWristThenShoulder(ArmState.STOW),
- Commands.waitUntil(() -> arm.getPosition().at(ArmState.STOW)).andThen(intake.in()));
+ Commands.waitUntil(() -> arm.getPosition().at(ArmState.STOW)).withTimeout(2.0).andThen(intake.in())).withTimeout(seconds);
}
public Command readyShoot() {
+ double seconds = 3.0;
+
return Commands.parallel(
- Commands.waitUntil(intake::isNotStowed).andThen(arm.moveShoulderThenWrist(ArmState.SHOOT)),
- intake.out());
+ Commands.waitUntil(intake::isNotStowed).andThen(arm.moveWrist(ArmState.SHOOT)),
+ intake.out()).withTimeout(seconds);
}
public Command shootNote() {
diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java
index caa901c..24297e3 100644
--- a/src/main/java/frc/robot/climber/Climber.java
+++ b/src/main/java/frc/robot/climber/Climber.java
@@ -2,6 +2,8 @@
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
import frc.robot.climber.ClimberConstants.ElevatorConstants;
@@ -14,16 +16,27 @@ public class Climber extends Subsystem {
private static Climber instance = null;
/** Elevator. */
- private final ElevatorIO elevator;
+ private final ElevatorIO eastElevator;
/** Elevator values. */
- private final ElevatorIOValues elevatorValues = new ElevatorIOValues();
+ private final ElevatorIOValues eastElevatorValues = new ElevatorIOValues();
+
+ /** Elevator. */
+ private final ElevatorIO westElevator;
+
+ /** Elevator values. */
+ private final ElevatorIOValues westElevatorValues = new ElevatorIOValues();
/** Creates a new instance of the climber subsystem. */
private Climber() {
- elevator = ClimberFactory.createElevator();
+ westElevator = new ElevatorIOSim(); // TODO
+ eastElevator = new ElevatorIOSim(); // TODO
+
+ westElevator.configure();
+ eastElevator.configure();
- elevator.setPosition(ElevatorConstants.MIN_HEIGHT);
+ westElevator.setPosition(ElevatorConstants.MIN_HEIGHT);
+ eastElevator.setPosition(ElevatorConstants.MIN_HEIGHT);
}
/**
@@ -40,12 +53,58 @@ public static Climber getInstance() {
}
@Override
- public void periodic() {}
+ public void periodic() {
+ westElevator.update(westElevatorValues);
+ eastElevator.update(eastElevatorValues);
+ }
@Override
public void addToShuffleboard(ShuffleboardTab tab) {
- ShuffleboardLayout elevator = Telemetry.addColumn(tab, "Elevator");
+ ShuffleboardLayout west = Telemetry.addColumn(tab, "West");
+
+ west.addDouble("Position (m)", () -> westElevatorValues.positionMeters);
+
+ ShuffleboardLayout east = Telemetry.addColumn(tab, "East");
+
+ east.addDouble("Position (m)", () -> eastElevatorValues.positionMeters);
+ }
+
+ public boolean westAtTop() {
+ return westElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT;
+ }
+
+ public boolean westAtBottom() {
+ return westElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT;
+ }
+
+ public boolean eastAtTop() {
+ return eastElevatorValues.positionMeters >= ElevatorConstants.MAX_HEIGHT;
+ }
+
+ public boolean eastAtBottom() {
+ return eastElevatorValues.positionMeters <= ElevatorConstants.MIN_HEIGHT;
+ }
+
+ public void stop() {
+ westElevator.setVoltage(0);
+ eastElevator.setVoltage(0);
+ }
+
+ public Command up() {
+ return Commands.parallel(
+ Commands.run(() -> westElevator.setVoltage(ElevatorConstants.UP_VOLTAGE))
+ .until(this::westAtTop),
+ Commands.run(() -> eastElevator.setVoltage(ElevatorConstants.UP_VOLTAGE))
+ .until(this::eastAtTop))
+ .finallyDo(this::stop);
+ }
- elevator.addDouble("Position (m)", () -> elevatorValues.positionMeters);
+ public Command down() {
+ return Commands.parallel(
+ Commands.run(() -> westElevator.setVoltage(ElevatorConstants.DOWN_VOLTAGE))
+ .until(this::westAtBottom),
+ Commands.run(() -> eastElevator.setVoltage(ElevatorConstants.DOWN_VOLTAGE))
+ .until(this::eastAtBottom))
+ .finallyDo(this::stop);
}
}
diff --git a/src/main/java/frc/robot/climber/ClimberConstants.java b/src/main/java/frc/robot/climber/ClimberConstants.java
index aea37f2..afdc2ab 100644
--- a/src/main/java/frc/robot/climber/ClimberConstants.java
+++ b/src/main/java/frc/robot/climber/ClimberConstants.java
@@ -8,7 +8,7 @@ public class ClimberConstants {
/** Constants for the elevator. */
public static class ElevatorConstants {
/** Gearing between the elevator motor and drum. */
- public static final double GEARING = 1.0;
+ public static final double GEARING = 25.0;
/** Mass of the elevator carriage in kilograms. */
// TODO Does not include the mass of two WCP-0418 parts
@@ -23,5 +23,11 @@ public static class ElevatorConstants {
/** Maximum height of the elevator in meters. */
public static final double MAX_HEIGHT = Units.inchesToMeters(28);
+
+ /** Voltage to be applied while going up. */
+ public static final double UP_VOLTAGE = 2;
+
+ /** Voltage to be applied wihle going down. */
+ public static final double DOWN_VOLTAGE = 1;
}
}
diff --git a/src/main/java/frc/robot/climber/ClimberFactory.java b/src/main/java/frc/robot/climber/ClimberFactory.java
index ef46b10..a78dc4b 100644
--- a/src/main/java/frc/robot/climber/ClimberFactory.java
+++ b/src/main/java/frc/robot/climber/ClimberFactory.java
@@ -1,5 +1,10 @@
package frc.robot.climber;
+import frc.lib.CAN;
+import frc.robot.Robot;
+import frc.robot.RobotConstants;
+import frc.robot.RobotConstants.Subsystem;
+
/** Helper class for creating hardware for the climber subsystem. */
public class ClimberFactory {
@@ -8,11 +13,10 @@ public class ClimberFactory {
*
* @return an elevator.
*/
- public static ElevatorIO createElevator() {
- // TODO
- // if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.CLIMBER)) {
- // return new ElevatorIONeo();
- // }
+ public static ElevatorIO createElevator(CAN can, boolean inverted) {
+ if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.CLIMBER)) {
+ return new ElevatorIOSparkMax(can, inverted);
+ }
return new ElevatorIOSim();
}
diff --git a/src/main/java/frc/robot/climber/ElevatorIO.java b/src/main/java/frc/robot/climber/ElevatorIO.java
index 82c5e88..c7d1967 100644
--- a/src/main/java/frc/robot/climber/ElevatorIO.java
+++ b/src/main/java/frc/robot/climber/ElevatorIO.java
@@ -27,9 +27,9 @@ public static class ElevatorIOValues {
public void setPosition(double positionMeters);
/**
- * Sets the elevator's setpoint.
+ * Sets the elevator's voltage.
*
- * @param positionMeters the elevator's setpoint.
+ * @param volts the elevator's voltage.
*/
- public void setSetpoint(double positionMeters);
+ public void setVoltage(double volts);
}
diff --git a/src/main/java/frc/robot/climber/ElevatorIOSim.java b/src/main/java/frc/robot/climber/ElevatorIOSim.java
index 5cd4fb3..2c50f4a 100644
--- a/src/main/java/frc/robot/climber/ElevatorIOSim.java
+++ b/src/main/java/frc/robot/climber/ElevatorIOSim.java
@@ -1,7 +1,5 @@
package frc.robot.climber;
-import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
import frc.robot.RobotConstants;
@@ -14,10 +12,6 @@ public class ElevatorIOSim implements ElevatorIO {
private final ElevatorSim elevatorSim;
- private final PIDController feedback;
-
- private final ElevatorFeedforward feedforward;
-
public ElevatorIOSim() {
motor = DCMotor.getNEO(1);
@@ -29,14 +23,8 @@ public ElevatorIOSim() {
ElevatorConstants.DRUM_DIAMETER,
ElevatorConstants.MIN_HEIGHT,
ElevatorConstants.MAX_HEIGHT,
- true,
+ false,
ElevatorConstants.MIN_HEIGHT);
-
- // TODO
- feedback = new PIDController(1.0, 0.0, 0.0);
-
- // TODO
- feedforward = new ElevatorFeedforward(0, 0, 0);
}
@Override
@@ -55,11 +43,7 @@ public void setPosition(double positionMeters) {
}
@Override
- public void setSetpoint(double positionMeters) {
- double feedbackVolts = feedback.calculate(elevatorSim.getPositionMeters(), positionMeters);
-
- double feedforwardVolts = feedforward.calculate(elevatorSim.getVelocityMetersPerSecond());
-
- elevatorSim.setInputVoltage(feedbackVolts + feedforwardVolts);
+ public void setVoltage(double volts) {
+ elevatorSim.setInputVoltage(volts);
}
}
diff --git a/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java
new file mode 100644
index 0000000..0c205d4
--- /dev/null
+++ b/src/main/java/frc/robot/climber/ElevatorIOSparkMax.java
@@ -0,0 +1,58 @@
+package frc.robot.climber;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import frc.lib.CAN;
+import frc.lib.Configurator;
+import frc.robot.climber.ClimberConstants.ElevatorConstants;
+
+public class ElevatorIOSparkMax implements ElevatorIO {
+
+ private final CANSparkMax sparkMax;
+
+ private final boolean isInverted;
+
+ private static final double METERS_PER_ROTATION = 1.0;
+
+ public ElevatorIOSparkMax(CAN can, boolean invert) {
+ sparkMax = new CANSparkMax(can.id(), MotorType.kBrushless);
+
+ isInverted = invert;
+ }
+
+ @Override
+ public void configure() {
+ sparkMax.setInverted(isInverted);
+
+ Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake));
+
+ Configurator.configureStatusFrames(sparkMax);
+ }
+
+ @Override
+ public void update(ElevatorIOValues values) {
+ values.positionMeters = getPositionRotations() * METERS_PER_ROTATION;
+ }
+
+ /**
+ * Gets the absolute position in rotations.
+ *
+ * @return the absolute position in rotations.
+ */
+ private double getPositionRotations() {
+ return sparkMax.getEncoder().getPosition() / ElevatorConstants.GEARING;
+ }
+
+ @Override
+ public void setPosition(double positionMeters) {
+ double rotations = positionMeters / METERS_PER_ROTATION;
+
+ sparkMax.getEncoder().setPosition(rotations);
+ }
+
+ @Override
+ public void setVoltage(double volts) {
+ sparkMax.setVoltage(volts);
+ }
+}
diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java
index a4b5301..90b1070 100644
--- a/src/main/java/frc/robot/intake/Intake.java
+++ b/src/main/java/frc/robot/intake/Intake.java
@@ -140,11 +140,11 @@ private boolean atPivotSetpoint() {
}
public Command out() {
- return Commands.race(pivotTo(PivotMotorConstants.MINIMUM_ANGLE), Commands.waitSeconds(2.0));
+ return pivotTo(PivotMotorConstants.MINIMUM_ANGLE).withTimeout(3.0);
}
public Command in() {
- return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE);
+ return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE).withTimeout(3.0);
}
private Command pivotTo(Rotation2d angle) {
diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java
index 1eb80db..e3d2870 100644
--- a/src/main/java/frc/robot/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/intake/IntakeConstants.java
@@ -43,7 +43,7 @@ public static class PivotMotorConstants {
public static final Rotation2d OUT_ANGLE = Rotation2d.fromDegrees(0);
/** Pivot motor's tolerance. */
- public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(4.0);
+ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(8.0);
/** Maximum speed of the pivot in rotations per second. */
public static final double MAXIMUM_SPEED = 1;
diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java
index f17bc53..ad1a451 100644
--- a/src/main/java/frc/robot/odometry/Odometry.java
+++ b/src/main/java/frc/robot/odometry/Odometry.java
@@ -12,6 +12,7 @@
import edu.wpi.first.wpilibj2.command.Commands;
import frc.lib.Subsystem;
import frc.lib.Telemetry;
+import frc.lib.AllianceFlipHelper;
import frc.robot.odometry.GyroscopeIO.GyroscopeIOValues;
import frc.robot.swerve.Swerve;
import java.util.function.Supplier;
@@ -135,7 +136,14 @@ public void setRotation(Rotation2d rotation) {
* @return a command that zeroes the rotation of the robot.
*/
public Command tare() {
- return Commands.runOnce(() -> setRotation(Rotation2d.fromDegrees(0)));
+ return Commands.runOnce(() -> {
+ // if (AllianceFlipHelper.shouldFlip()) {
+ // gyroscope.setYaw(0.5);
+ // } else {
+ // gyroscope.setYaw(0.0);
+ // }
+ gyroscope.setYaw(0.0);
+ });
}
/**
diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java b/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java
index 6d19f1b..81c1795 100644
--- a/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java
+++ b/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java
@@ -18,7 +18,7 @@ public SerializerMotorIOSparkMax() {
public void configure() {
Configurator.configureREV(sparkMax::restoreFactoryDefaults);
- Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(30));
+ Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(40));
Configurator.configureStatusFrames(sparkMax);
}
diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java
index ee0b5b4..742a630 100644
--- a/src/main/java/frc/robot/shooter/Shooter.java
+++ b/src/main/java/frc/robot/shooter/Shooter.java
@@ -10,7 +10,6 @@
import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues;
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
import frc.robot.shooter.ShooterConstants.SerializerConstants;
-import frc.robot.shooter.ShooterConstants.ShooterCommandConstants;
/** Subsystem class for the shooter subsystem. */
public class Shooter extends Subsystem {
@@ -122,7 +121,9 @@ public Command spin() {
* @return a command that shoots a note.
*/
public Command autoShoot() {
- return Commands.deadline(
- Commands.waitSeconds(ShooterCommandConstants.PRE_SHOOT_DELAY).andThen(serialize()), spin());
+ return Commands.parallel(
+ serialize().beforeStarting(Commands.waitSeconds(1.0)),
+ spin()
+ ).withTimeout(3.0);
}
}
diff --git a/src/main/java/frc/robot/swerve/DriveCommand.java b/src/main/java/frc/robot/swerve/DriveCommand.java
index 111ec8d..831015b 100644
--- a/src/main/java/frc/robot/swerve/DriveCommand.java
+++ b/src/main/java/frc/robot/swerve/DriveCommand.java
@@ -97,22 +97,12 @@ public void execute() {
headingFeedback.calculate(poseHeading.getRotations(), headingSetpoint.position);
}
- ChassisSpeeds chassisSpeeds;
-
- if (request.isRobotCentric()) {
- chassisSpeeds =
- new ChassisSpeeds(
- translationVelocityMetersPerSecond.getX(),
- translationVelocityMetersPerSecond.getY(),
- Units.rotationsToRadians(omegaRotationsPerSecond));
- } else {
- chassisSpeeds =
- ChassisSpeeds.fromFieldRelativeSpeeds(
- translationVelocityMetersPerSecond.getX(),
- translationVelocityMetersPerSecond.getY(),
- Units.rotationsToRadians(omegaRotationsPerSecond),
- poseHeading);
- }
+ ChassisSpeeds chassisSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ translationVelocityMetersPerSecond.getX(),
+ translationVelocityMetersPerSecond.getY(),
+ Units.rotationsToRadians(omegaRotationsPerSecond),
+ poseHeading);
chassisSpeeds.vxMetersPerSecond =
MathUtil.clamp(
diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java
index 8007c1f..a48f30e 100644
--- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java
+++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java
@@ -35,9 +35,8 @@ public DriveMotorIOTalonFX(CAN driveMotorCAN) {
talonFXBaseConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING;
- // TODO Copied from Citrus Circuits 2023 release
talonFXBaseConfig.CurrentLimits =
- new MotorCurrentLimits(300.0, 30.0, 60.0, 1.0).asCurrentLimitsConfigs();
+ new MotorCurrentLimits(80.0, 30.0, 60.0, 0.1).asCurrentLimitsConfigs();
positionRotations = talonFX.getPosition();
velocityRotationsPerSecond = talonFX.getVelocity();
diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java
index b9e1230..2605ea9 100644
--- a/src/main/java/frc/robot/swerve/DriveRequest.java
+++ b/src/main/java/frc/robot/swerve/DriveRequest.java
@@ -39,6 +39,7 @@ public static DriveRequest fromController(CommandXboxController controller) {
boolean aligningRequested = controller.rightTrigger().getAsBoolean();
double translationX = -controller.getLeftY();
+
double translationY = -controller.getLeftX();
double translationMagnitude = Math.hypot(translationX, translationY);
diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java
index 44e48b1..ed1f07f 100644
--- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java
+++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java
@@ -43,7 +43,7 @@ public void configure() {
talonFXPIDFConfig.ClosedLoopGeneral.ContinuousWrap = true;
talonFXPIDFConfig.CurrentLimits =
- new MotorCurrentLimits(0.0, 20.0, 30.0, 1.0).asCurrentLimitsConfigs();
+ new MotorCurrentLimits(40.0, 20.0, 30.0, 0.1).asCurrentLimitsConfigs();
talonFXPIDFConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING;
diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java
index 87cdb90..890cb51 100644
--- a/src/main/java/frc/robot/swerve/SwerveConstants.java
+++ b/src/main/java/frc/robot/swerve/SwerveConstants.java
@@ -67,28 +67,28 @@ public static class MK4iConstants {
new SwerveModuleConfig(
new SwerveModuleCAN(11, 10, 12, SWERVE_BUS),
new Translation2d(X_OFFSET, Y_OFFSET),
- Rotation2d.fromRotations(-0.304688));
+ Rotation2d.fromRotations(-0.027100));
/** Module configuration for the north east swerve module. */
public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG =
new SwerveModuleConfig(
new SwerveModuleCAN(2, 1, 3, SWERVE_BUS),
new Translation2d(X_OFFSET, -Y_OFFSET),
- Rotation2d.fromRotations(-0.433838));
+ Rotation2d.fromRotations(-0.429688));
/** Module configuration for the south east swerve module. */
public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG =
new SwerveModuleConfig(
new SwerveModuleCAN(5, 4, 6, SWERVE_BUS),
new Translation2d(-X_OFFSET, -Y_OFFSET),
- Rotation2d.fromRotations(-0.104980));
+ Rotation2d.fromRotations(-0.097168));
/** Module configuration for the south west swerve module. */
public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG =
new SwerveModuleConfig(
new SwerveModuleCAN(8, 7, 9, SWERVE_BUS),
new Translation2d(-X_OFFSET, Y_OFFSET),
- Rotation2d.fromRotations(-0.290771));
+ Rotation2d.fromRotations(-0.309814));
/**
* Calculates the maximum attainable open loop speed in meters per second.