diff --git a/src/main/deploy/pathplanner/autos/Left Shoot.auto b/src/main/deploy/pathplanner/autos/Left Shoot.auto index 89e9dc7..c13bced 100644 --- a/src/main/deploy/pathplanner/autos/Left Shoot.auto +++ b/src/main/deploy/pathplanner/autos/Left Shoot.auto @@ -27,6 +27,36 @@ "data": { "name": "shoot close" } + }, + { + "type": "path", + "data": { + "pathName": "Left to Outpost" + } + }, + { + "type": "named", + "data": { + "name": "stop spindexer" + } + }, + { + "type": "path", + "data": { + "pathName": "trough to shot" + } + }, + { + "type": "named", + "data": { + "name": "spinup" + } + }, + { + "type": "named", + "data": { + "name": "shoot N jimmy" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/left to center.auto b/src/main/deploy/pathplanner/autos/left to center.auto new file mode 100644 index 0000000..6722e6f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/left to center.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake down" + } + }, + { + "type": "path", + "data": { + "pathName": "left start to center" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to Shoot.path b/src/main/deploy/pathplanner/paths/Center to Shoot.path index 5eb9074..4e517d8 100644 --- a/src/main/deploy/pathplanner/paths/Center to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Center to Shoot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 3f475e5..9678040 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Left to Outpost.path b/src/main/deploy/pathplanner/paths/Left to Outpost.path index f7d6135..b4b3765 100644 --- a/src/main/deploy/pathplanner/paths/Left to Outpost.path +++ b/src/main/deploy/pathplanner/paths/Left to Outpost.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.9872360616844604, - "y": 4.599857651245552 + "x": 3.2515736040609142, + "y": 4.914375634517767 }, "prevControl": null, "nextControl": { - "x": 2.1695373665480435, - "y": 5.019466192170819 + "x": 2.4338749089244973, + "y": 5.333984175443034 }, "isLocked": false, "linkedName": "left close" }, { "anchor": { - "x": 1.1043772241992889, - "y": 5.815646500593119 + "x": 0.77458883248731, + "y": 5.927269035532995 }, "prevControl": { - "x": 2.7074970344009497, - "y": 5.998552787663108 + "x": 2.3777086426889733, + "y": 6.110175322602984 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "trough" } ], "rotationTargets": [ @@ -34,12 +34,33 @@ "rotationDegrees": 180.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.3963599595551063, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake Start", + "waypointRelativePos": 0.09706774519716882, + "endWaypointRelativePos": null, + "command": null + } + ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -47,13 +68,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 178.53119928561412 + "rotation": 178.80651057601818 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -22.619864948040433 + "rotation": -31.15930450834445 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path b/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path index 954aba4..486b7c7 100644 --- a/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path +++ b/src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path @@ -69,7 +69,7 @@ }, "prevControl": { "x": 8.08578683847011, - "y": 0.49077040160283747 + "y": 0.4907704016028374 }, "nextControl": { "x": 8.318287488908608, @@ -130,8 +130,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path b/src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path index 68eff51..b8c1452 100644 --- a/src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path +++ b/src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 24.304549265936604 + "rotation": 24.304549265936608 }, "reversed": false, "folder": "Right Outpost", diff --git a/src/main/deploy/pathplanner/paths/Start to outpost.path b/src/main/deploy/pathplanner/paths/Start to outpost.path index ed9134f..8b242d5 100644 --- a/src/main/deploy/pathplanner/paths/Start to outpost.path +++ b/src/main/deploy/pathplanner/paths/Start to outpost.path @@ -20,7 +20,7 @@ "y": 0.668228980317108 }, "prevControl": { - "x": 1.6987119856944102, + "x": 1.6987119856944104, "y": 1.0414132379199703 }, "nextControl": null, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/left start to center.path b/src/main/deploy/pathplanner/paths/left start to center.path new file mode 100644 index 0000000..377be13 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/left start to center.path @@ -0,0 +1,132 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.573857868020305, + "y": 6.3692588832487305 + }, + "prevControl": null, + "nextControl": { + "x": 3.1594923857868027, + "y": 6.424507614213196 + }, + "isLocked": false, + "linkedName": "start left" + }, + { + "anchor": { + "x": 2.634629441624366, + "y": 5.558944162436549 + }, + "prevControl": { + "x": 2.4136345177664977, + "y": 5.908852791878173 + }, + "nextControl": { + "x": 2.895419769065523, + "y": 5.146026143988051 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.522294416243654, + "y": 5.558944162436549 + }, + "prevControl": { + "x": 3.5095162657293457, + "y": 5.533304209258972 + }, + "nextControl": { + "x": 6.704619289340102, + "y": 5.614192893401015 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.478101522849547, + "y": 7.244030456855094 + }, + "prevControl": { + "x": 6.765542310662505, + "y": 7.465169522706244 + }, + "nextControl": { + "x": 8.012172588839395, + "y": 7.078284263961693 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.671472081218274, + "y": 4.7670456852791885 + }, + "prevControl": { + "x": 7.726720812182741, + "y": 7.639979695431472 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5636363636363326, + "rotationDegrees": -45.0 + }, + { + "waypointRelativePos": 3.0318181818182266, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.6208291203235685, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake Start", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -89.09061955080092 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/start to score left.path b/src/main/deploy/pathplanner/paths/start to score left.path index a18a33f..e908ceb 100644 --- a/src/main/deploy/pathplanner/paths/start to score left.path +++ b/src/main/deploy/pathplanner/paths/start to score left.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.697342823250297, - "y": 6.0 + "x": 3.573857868020305, + "y": 6.3692588832487305 }, "prevControl": null, "nextControl": { - "x": 2.922680901542112, - "y": 5.890960854092527 + "x": 2.7991959463121194, + "y": 6.260219737341258 }, "isLocked": false, - "linkedName": null + "linkedName": "start left" }, { "anchor": { - "x": 2.9872360616844604, - "y": 4.599857651245552 + "x": 3.2515736040609142, + "y": 4.914375634517767 }, "prevControl": { - "x": 3.1809015421115063, - "y": 5.116298932384343 + "x": 3.44523908448796, + "y": 5.430816915656558 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -22.619864948040433 + "rotation": -31.15930450834445 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/trough to shot.path b/src/main/deploy/pathplanner/paths/trough to shot.path new file mode 100644 index 0000000..7eab232 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/trough to shot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.77458883248731, + "y": 5.927269035532995 + }, + "prevControl": null, + "nextControl": { + "x": 1.7745888324873098, + "y": 5.927269035532995 + }, + "isLocked": false, + "linkedName": "trough" + }, + { + "anchor": { + "x": 3.2515736040609142, + "y": 4.914375634517767 + }, + "prevControl": { + "x": 2.6254213197969554, + "y": 5.420822335025381 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "left close" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.15930450834445 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 178.80651057601818 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 1d43a28..3cad5ec 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -6,8 +6,8 @@ "Right Outpost" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 1.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2f3bfe..2fc10c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,8 @@ import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.events.EventTrigger; +import com.pathplanner.lib.path.EventMarker; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; @@ -19,6 +21,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 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.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -71,7 +74,7 @@ public class RobotContainer { configureNamedCommands(); - vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); + //vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { Logger.recordOutput( "Vision/" + vp.cameraName() + "/Pose", @@ -333,7 +336,7 @@ public class RobotContainer { NamedCommands.registerCommand( "intake down", - intakePivot.manualSpeed(()->-0.75) + intakePivot.manualSpeed(()->0.75) .withTimeout(1) ); @@ -345,7 +348,33 @@ public class RobotContainer { spindexer.spinToShooter() .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)) .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10))) - .withTimeout(3)); + .withTimeout(3).andThen(spindexer.instantaneousStop())); + + // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn()); + + new EventTrigger("Intake Start") + .onTrue(intakeRoller.runIn()); + + NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop()); + + NamedCommands.registerCommand("jimmy", + Commands.repeatingSequence( + intakePivot.manualSpeed(() -> -0.75).withTimeout(0.2) + .andThen(intakePivot.manualSpeed(() -> 0.75).withTimeout(0.2)) + ) + ); + + NamedCommands.registerCommand("shoot N jimmy", + Commands.parallel( + Commands.repeatingSequence( + intakePivot.manualSpeed(() -> -0.75).withTimeout(0.5), + intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5) + ), + spindexer.spinToShooter() + .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), + hood.trackToAngle(() -> Units.degreesToRadians(10))) + + ).withTimeout(3).andThen(spindexer.instantaneousStop())); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/SpindexerConstants.java b/src/main/java/frc/robot/constants/SpindexerConstants.java index 8036523..006c3a5 100644 --- a/src/main/java/frc/robot/constants/SpindexerConstants.java +++ b/src/main/java/frc/robot/constants/SpindexerConstants.java @@ -12,9 +12,9 @@ public class SpindexerConstants { public static final int kSpindexerMotorCANID = 0; public static final int kFeederMotorCANID = 4; - public static final int kSpindexerStatorCurrentLimit = 110; - public static final int kSpindexerSupplyCurrentLimit = 60; - public static final int kFeederCurrentLimit = 40; + public static final int kSpindexerStatorCurrentLimit = 95; + public static final int kSpindexerSupplyCurrentLimit = 50; + public static final int kFeederCurrentLimit = 30; public static final double kSpindexerSpeed = 1; public static final double kFeederSpeed = 1; diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java index 1180b4f..14f9428 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -80,6 +80,13 @@ public class IntakePivot extends SubsystemBase { }); } + // public Command jimmy(){ + // return run(() -> { + // leftMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier); + // rightMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier); + // }) + // } + public Command stop() { return manualSpeed(() -> 0); } diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index b57c0e3..b64cf2e 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -59,5 +59,12 @@ public class Spindexer extends SubsystemBase { feederMotor.set(0); }); } + + public Command instantaneousStop() { + return runOnce(() -> { + spindexerMotor.setControl(spindexerMotorOutput.withOutput(0)); + feederMotor.set(0); + }); + } }