--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "path",
+ "data": {
+ "pathName": "#1 Left under the trench"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "#2 Left(No SOTM) under the trench"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "#3(No SOTM) Left under the trench"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
"waypoints": [
{
"anchor": {
- "x": 3.566008403832779,
- "y": 7.457340509007208
+ "x": 4.332135231316728,
+ "y": 7.623190984578885
},
"prevControl": null,
"nextControl": {
- "x": 4.521985876085326,
- "y": 7.480627842977369
+ "x": 5.332740213523133,
+ "y": 7.3757295373665475
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 6.74844449371633,
- "y": 7.323051878639669
+ "x": 7.841663907284769,
+ "y": 7.354211150652431
},
"prevControl": {
- "x": 6.2291917985744085,
- "y": 7.524060964793236
+ "x": 7.0217100817524996,
+ "y": 7.365924776731462
},
"nextControl": {
- "x": 7.0431955211446535,
- "y": 7.208950141238001
+ "x": 8.594807442278832,
+ "y": 7.3434519572953745
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 7.47952715610716,
- "y": 6.691750705058882
+ "x": 8.67884934756821,
+ "y": 6.21373665480427
},
"prevControl": {
- "x": 7.292883599447913,
- "y": 7.06382367183081
+ "x": 8.72006915663984,
+ "y": 6.49478080756538
},
"nextControl": {
- "x": 7.8379030178105324,
- "y": 5.97733027463995
+ "x": 8.560498220640572,
+ "y": 5.406797153024911
},
"isLocked": false,
"linkedName": null
"y": 4.4778559602649
},
"prevControl": {
- "x": 7.841663907284769,
- "y": 5.5015066225165565
+ "x": 8.721886120996441,
+ "y": 5.148576512455516
},
"nextControl": null,
"isLocked": false,
],
"rotationTargets": [
{
- "waypointRelativePos": 0.5,
- "rotationDegrees": 0.0
- },
- {
- "waypointRelativePos": 1.2283611599297022,
- "rotationDegrees": -51.65199999999999
+ "waypointRelativePos": 1.6,
+ "rotationDegrees": -104.27189939195472
}
],
"constraintZones": [
{
"name": "Constraints Zone",
- "minWaypointRelativePos": 1.908695652173902,
+ "minWaypointRelativePos": 1.95,
"maxWaypointRelativePos": 2.759480519480524,
"constraints": {
- "maxVelocity": 1.5,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- },
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 3.75,
- "maxWaypointRelativePos": 3.75,
- "constraints": {
- "maxVelocity": 2.0,
- "maxAcceleration": 3.0,
- "maxAngularVelocity": 540.0,
- "maxAngularAcceleration": 720.0,
- "nominalVoltage": 12.0,
- "unlimited": false
- }
- },
- {
- "name": "Constraints Zone",
- "minWaypointRelativePos": 3.75,
- "maxWaypointRelativePos": 3.9,
- "constraints": {
- "maxVelocity": 1.0,
+ "maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
{
"name": "Constraints Zone",
"minWaypointRelativePos": 0,
- "maxWaypointRelativePos": 1.0565217391304353,
+ "maxWaypointRelativePos": 2.0652418447694045,
"constraints": {
- "maxVelocity": 5.0,
- "maxAcceleration": 5.0,
+ "maxVelocity": 6.0,
+ "maxAcceleration": 7.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
},
"goalEndState": {
"velocity": 0,
- "rotation": -90.0
+ "rotation": -110.0
},
"reversed": false,
"folder": "Left Autos",
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 7.842,
+ "y": 4.478
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 8.226963226571769,
+ "y": 5.202372479240807
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.816903914590748,
+ "y": 7.41876631079478
+ },
+ "prevControl": {
+ "x": 7.157170659761689,
+ "y": 7.27343618180034
+ },
+ "nextControl": {
+ "x": 4.030877817319099,
+ "y": 7.612431791221827
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.0302728351126933,
+ "y": 7.042194543297747
+ },
+ "prevControl": {
+ "x": 3.3853262158956117,
+ "y": 7.300415183867142
+ },
+ "nextControl": {
+ "x": 2.6936266796220245,
+ "y": 6.797360975668169
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.18,
+ "y": 5.966
+ },
+ "prevControl": {
+ "x": 2.224810422787547,
+ "y": 6.241444208960923
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.5,
+ "rotationDegrees": 40.0
+ },
+ {
+ "waypointRelativePos": 0.927175843694493,
+ "rotationDegrees": -179.587
+ },
+ {
+ "waypointRelativePos": 1.4422735346358764,
+ "rotationDegrees": 180.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0,
+ "maxWaypointRelativePos": 1.0565217391304353,
+ "constraints": {
+ "maxVelocity": 5.0,
+ "maxAcceleration": 7.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "reversed": false,
+ "folder": "Left Autos",
+ "idealStartingState": {
+ "velocity": 0.0,
+ "rotation": -110.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 2.18,
+ "y": 5.966
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 0.9893708609271523,
+ "y": 6.002997790234502
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.6847686832740218,
+ "y": 5.966
+ },
+ "prevControl": {
+ "x": 1.3625978647686838,
+ "y": 5.944756820877817
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0,
+ "maxWaypointRelativePos": 1.0,
+ "constraints": {
+ "maxVelocity": 1.5,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "reversed": false,
+ "folder": "Left Autos",
+ "idealStartingState": {
+ "velocity": 0.0,
+ "rotation": 180.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": [
- "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.5967,\"length\":0.241},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
+ "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.5967,\"length\":0.254},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
]
}
\ No newline at end of file
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.PS5Controller;
import edu.wpi.first.wpilibj.RobotController;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.subsystems.spindexer.SpindexerConstants;
e.printStackTrace();
}
if(turret != null){
- //turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
+ turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
}
drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
break;
}
if (intake != null && spindexer != null){
- NamedCommands.registerCommand("Intake", new ParallelCommandGroup(
+ NamedCommands.registerCommand("Spin Intake Rollers", new ParallelCommandGroup(
new InstantCommand(()->intake.spin(IntakeConstants.SPEED))
- //new InstantCommand(()-> spindexer.setSpindexer(SpindexerConstants.spindexerMaxPower))
+ ));
+ NamedCommands.registerCommand("Stop Intake Rollers", new ParallelCommandGroup(
+ new InstantCommand(()->intake.spinStop())
));
}
if (turret != null && drive != null && hood != null && shooter != null && spindexer != null){
NamedCommands.registerCommand("Auto shoot", new AutoShootCommand(turret, drive, hood, shooter, spindexer));
+ NamedCommands.registerCommand("Spin Spindexer", new InstantCommand(()-> spindexer.maxSpindexer(), spindexer));
+ NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(()-> spindexer.stopSpindexer()));
+ }
+
+ if (hood != null){
+ Command hoodDown = new InstantCommand(()-> {hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0);}, hood);
+ NamedCommands.registerCommand("Hood Down", new InstantCommand(()->{hoodDown.schedule();}));
+ NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(()-> {hoodDown.cancel();}));
}
+
if (linearClimb != null && drive != null){
NamedCommands.registerCommand("Climb", new ClimbDriveCommand(linearClimb, drive));
}
private double lastFilteredRad = 0.0;
private double lastRawSetpoint = 0.0;
- private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
+ private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
private boolean calibrating = false;
private Debouncer calibrateDebouncer = new Debouncer(0.5, DebounceType.kRising);