diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..6fceb12 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.88, + "robotLength": 0.88, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.03 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1S 2A.auto b/src/main/deploy/pathplanner/autos/Right 1S 2A.auto new file mode 100644 index 0000000..d2b8fb5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1S 2A.auto @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.1134340217398382, + "y": 6.525338178881486 + }, + "rotation": 42.79740183823424 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker Note Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Preloaded+Pickup" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Amp Handoff" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Note 1 to amp" + } + }, + { + "type": "named", + "data": { + "name": "Amp Note Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Leftmost Center Note" + } + }, + { + "type": "path", + "data": { + "pathName": "Note 1 to amp" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Amp.auto b/src/main/deploy/pathplanner/autos/Right 2 Amp.auto new file mode 100644 index 0000000..5b26dfc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Amp.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker Note Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Preloaded+Pickup" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Amp Handoff" + } + }, + { + "type": "path", + "data": { + "pathName": "Note 1 to amp" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Amp Note Shot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Amp.auto b/src/main/deploy/pathplanner/autos/Right 3 Amp.auto new file mode 100644 index 0000000..7834ebe --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Amp.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.1134340217398382, + "y": 6.525338178881486 + }, + "rotation": 33.13560954871888 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Preloaded+Pickup" + } + }, + { + "type": "path", + "data": { + "pathName": "Note 1 to amp" + } + }, + { + "type": "path", + "data": { + "pathName": "Leftmost Center Note" + } + }, + { + "type": "path", + "data": { + "pathName": "Note 1 to amp" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leftmost Center Note.path b/src/main/deploy/pathplanner/paths/Leftmost Center Note.path new file mode 100644 index 0000000..c76a48f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Leftmost Center Note.path @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.347017520241028, + "y": 6.993104356575341 + }, + "prevControl": null, + "nextControl": { + "x": 4.446268037830382, + "y": 7.110045900999739 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.071755505338439, + "y": 7.297152372084174 + }, + "prevControl": { + "x": 5.077752377722138, + "y": 7.262069908751458 + }, + "nextControl": { + "x": 6.725659674531382, + "y": 7.320231342764915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.63877220062539, + "y": 7.425788070951012 + }, + "prevControl": { + "x": 6.621380764133118, + "y": 7.460870534278332 + }, + "nextControl": { + "x": 8.656163637117663, + "y": 7.390705607623692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.545818145767345, + "y": 6.642279723305886 + }, + "prevControl": { + "x": 6.323179825848877, + "y": 7.12758713266797 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 172.50414236027015, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.85, + "maxWaypointRelativePos": 1.35, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -179.23610153906992, + "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/Note 1 to Speaker.path b/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path new file mode 100644 index 0000000..0fc3f94 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2654580294915574, + "y": 6.291455090032687 + }, + "prevControl": { + "x": 1.7098358983042754, + "y": 6.525338178881486 + }, + "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": -145.88552705465875, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 179.55484341368705, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note 1 to amp.path b/src/main/deploy/pathplanner/paths/Note 1 to amp.path new file mode 100644 index 0000000..caae895 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note 1 to amp.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.353014392638472, + "y": 6.993104356579084 + }, + "prevControl": null, + "nextControl": { + "x": 1.768306670516475, + "y": 7.004798511021523 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8384715971711143, + "y": 7.624588696470841 + }, + "prevControl": { + "x": 1.7950792079196904, + "y": 7.315863019194643 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -178.36342295838335, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Preloaded+Pickup.path b/src/main/deploy/pathplanner/paths/Preloaded+Pickup.path new file mode 100644 index 0000000..732c2b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Preloaded+Pickup.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1134340217398382, + "y": 6.525338178881486 + }, + "prevControl": null, + "nextControl": { + "x": 1.148516485067158, + "y": 7.191904982100564 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3179319293111518, + "y": 6.981410202136645 + }, + "prevControl": { + "x": 1.8033891338437944, + "y": 6.981410202136645 + }, + "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": 40.97173633351488, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d25aba..182028f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,11 +80,13 @@ public class RobotContainer { intake = new Intake(); - shooter = new Shooter(); + indexer = new Indexer(); + + shooter = new Shooter(indexer.getBeamBreak()); climber = new Climber(shooter.getShooterAngle()); - indexer = new Indexer(); + // An example Named Command, doesn't need to remain once we start actually adding real things // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index b1a5bcd..dfa14e3 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.util.Units; public final class DrivetrainConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds - public static final double kMaxSpeedMetersPerSecond = 4.6; + public static final double kMaxSpeedMetersPerSecond = 4.1; public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second public static final double kDirectionSlewRate = 2.4; // radians per second diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 000d22c..d9a1eb6 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -10,8 +10,7 @@ public class ShooterConstants { public static final double kShooterI = 0.0; public static final double kShooterD = 0.0; - public static final double kSShooterFF = 0.0; - public static final double kVShooterFF = 0.0; + public static final double kShooterFF = 0.0; public static final double kShooterPivotP = 0.0; public static final double kShooterPivotI = 0.0; diff --git a/src/main/java/frc/robot/constants/SwerveModuleConstants.java b/src/main/java/frc/robot/constants/SwerveModuleConstants.java index 5dd374f..339894f 100644 --- a/src/main/java/frc/robot/constants/SwerveModuleConstants.java +++ b/src/main/java/frc/robot/constants/SwerveModuleConstants.java @@ -49,6 +49,6 @@ public class SwerveModuleConstants { public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; - public static final int kDrivingMotorCurrentLimit = 60; // amps + public static final int kDrivingMotorCurrentLimit = 65; // amps public static final int kTurningMotorCurrentLimit = 30; // amps } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index e91bdbb..45c1f25 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -15,7 +15,6 @@ public class Climber extends SubsystemBase { private DoubleSupplier shooterAngle; - //TODO What tells the climber to stop climbing when it achieves the target height? public Climber(DoubleSupplier shooterAngle) { if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){ motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7b34064..c73c6c8 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -5,12 +5,12 @@ import java.util.function.DoubleSupplier; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.Command; @@ -28,54 +28,49 @@ public class Shooter extends SubsystemBase{ private Encoder pivotEncoder; - private PIDController topShooterPID; - private SimpleMotorFeedforward topShooterFF; - - private PIDController bottomShooterPID; - private SimpleMotorFeedforward bottomShooterFF; + private SparkPIDController topPID; + private SparkPIDController bottomPID; private PIDController shooterPivotPID; private ArmFeedforward shooterPivotFF; private DigitalInput shooterBeamBreak; - public Shooter(){ + public Shooter(BooleanSupplier indexerBeamBreak){ topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless); bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless); + shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); - topShooter.setSmartCurrentLimit(40); - topShooter.burnFlash(); - - bottomShooter.setSmartCurrentLimit(40); - bottomShooter.burnFlash(); - topEncoder = topShooter.getEncoder(); bottomEncoder = bottomShooter.getEncoder(); - shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); + topPID = topShooter.getPIDController(); + topPID.setFeedbackDevice(topEncoder); + bottomPID = bottomShooter.getPIDController(); + bottomPID.setFeedbackDevice(bottomEncoder); shooterPivot.setSmartCurrentLimit(50); - shooterPivot.burnFlash(); + topShooter.setSmartCurrentLimit(40); + bottomShooter.setSmartCurrentLimit(40); pivotEncoder = new Encoder(0, 1); pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion); shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel); - //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed - topShooterPID = new PIDController( - ShooterConstants.kShooterP, - ShooterConstants.kShooterI, - ShooterConstants.kShooterD - ); - bottomShooterPID = new PIDController( - ShooterConstants.kShooterP, - ShooterConstants.kShooterI, - ShooterConstants.kShooterD - ); + bottomShooter.burnFlash(); + shooterPivot.burnFlash(); + topShooter.burnFlash(); - topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF); - topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF); + topPID.setP(ShooterConstants.kShooterP); + topPID.setI(ShooterConstants.kShooterI); + topPID.setD(ShooterConstants.kShooterD); + topPID.setFF(ShooterConstants.kShooterFF); + + bottomPID.setP(ShooterConstants.kShooterP); + bottomPID.setI(ShooterConstants.kShooterI); + bottomPID.setI(ShooterConstants.kShooterD); + bottomPID.setFF(ShooterConstants.kShooterFF); shooterPivotPID = new PIDController( ShooterConstants.kShooterPivotP, @@ -83,28 +78,26 @@ public class Shooter extends SubsystemBase{ ShooterConstants.kShooterPivotD ); - shooterPivotFF = new ArmFeedforward( - ShooterConstants.kSShooterPivotFF, - ShooterConstants.kGShooterPivotFF, - ShooterConstants.kVShooterPivotFF - ); } public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){ return run(()-> { - shooterPivot.setIdleMode(IdleMode.kBrake); + angleAndSpeedControl(setpointAngle, topRPM, bottomRPM); + }); + } + + private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){ + shooterPivot.setIdleMode(IdleMode.kBrake); shooterPivot.setVoltage( shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + shooterPivotFF.calculate(setpointAngle, 0.0)); - topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM)); - - bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM)); - }); + topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity); + bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity); } - public Command recieveNote(){ + public Command ampHandoff(){ return run(() -> { shooterPivot.setIdleMode(IdleMode.kBrake); @@ -112,12 +105,10 @@ public class Shooter extends SubsystemBase{ shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0)); - if(shooterBeamBreak.get()){ - topShooter.set(0.25); - bottomShooter.set(0.25); + if(shooterBeamBreak.get() || shooterBeamBreak.get()){ + angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0); }else{ - topShooter.set(0.); - bottomShooter.set(0.); + angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0); } }); } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3c74146..ff15fab 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.2", + "version": "2024.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.2" + "version": "2024.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.2", + "version": "2024.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..a829581 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.1", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,