--- /dev/null
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "W5 Left Trench Start"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe3"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 2.5
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Reset Spindexer"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Hood Down"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Left Swipe2"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 1.0
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
+++ /dev/null
-{
- "version": "2025.0",
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "Stop Hood Down"
- }
- },
- {
- "type": "named",
- "data": {
- "name": null
- }
- }
- ]
- }
- },
- "resetOdom": true,
- "folder": null,
- "choreoAuto": false
-}
\ No newline at end of file
},
"prevControl": null,
"nextControl": {
- "x": 6.341153730575683,
- "y": 7.339067227185437
+ "x": 6.651298524925124,
+ "y": 7.607193541852019
},
"isLocked": false,
"linkedName": null
"y": 6.924453642384106
},
"prevControl": {
- "x": 8.031951473995347,
- "y": 7.123753266445548
+ "x": 8.025456588617843,
+ "y": 7.118663887579298
},
"nextControl": {
- "x": 8.333810115408626,
- "y": 6.725154018322663
+ "x": 8.456464624342907,
+ "y": 6.586940219527121
},
"isLocked": false,
"linkedName": null
"y": 5.668485099337747
},
"prevControl": {
- "x": 8.618618682530846,
- "y": 5.7310165129163355
+ "x": 8.453398729051012,
+ "y": 5.774889075676877
},
"nextControl": {
- "x": 7.627292843251787,
- "y": 5.626913168266943
+ "x": 7.668717493102982,
+ "y": 5.580825559331089
},
"isLocked": false,
"linkedName": null
"y": 6.27831953642384
},
"prevControl": {
- "x": 7.433678730378213,
- "y": 5.997765220428201
+ "x": 7.455955837425071,
+ "y": 6.060525942967513
},
"nextControl": {
- "x": 7.0916784354147495,
- "y": 6.784302377129385
+ "x": 7.167700799412434,
+ "y": 6.495691704464595
},
"isLocked": false,
"linkedName": null
"y": 7.408007117437722
},
"prevControl": {
- "x": 5.877242530931041,
- "y": 7.381065257813595
+ "x": 5.861909734801985,
+ "y": 7.3911421043858505
},
"nextControl": {
- "x": 4.284691670946883,
- "y": 7.543120509860877
+ "x": 3.5628490080812623,
+ "y": 7.546590977304791
},
"isLocked": false,
"linkedName": null
"y": 7.5
},
"prevControl": {
- "x": 4.397534345753931,
- "y": 7.5120104256577624
+ "x": 4.366224335486623,
+ "y": 7.4554835538530675
},
"nextControl": null,
"isLocked": false,
{
"name": "Constraints Zone",
"minWaypointRelativePos": 0.7761529808773961,
- "maxWaypointRelativePos": 1.642294713160863,
+ "maxWaypointRelativePos": 2.8908886389201145,
"constraints": {
- "maxVelocity": 1.0,
- "maxAcceleration": 2.0,
+ "maxVelocity": 1.67,
+ "maxAcceleration": 1.5,
"maxAngularVelocity": 200.0,
"maxAngularAcceleration": 150.0,
"nominalVoltage": 12.0,
}
],
"pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Stop Hood Down",
- "waypointRelativePos": 4.968503937007888,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Stop Hood Down"
- }
- }
- },
- {
- "name": "Start Spindexer",
- "waypointRelativePos": 5.0,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Start Spindexer"
- }
- }
- }
- ],
+ "eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 2.5,
--- /dev/null
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 4.5,
+ "y": 7.623
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 6.25,
+ "y": 7.623
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.999810201660737,
+ "y": 5.890960854092527
+ },
+ "prevControl": {
+ "x": 6.114457884162435,
+ "y": 6.884367069543684
+ },
+ "nextControl": {
+ "x": 5.907758101386918,
+ "y": 5.093342214431054
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 4.449228944246738
+ },
+ "prevControl": {
+ "x": 5.653713425517404,
+ "y": 4.619355154982582
+ },
+ "nextControl": {
+ "x": 6.687014708632405,
+ "y": 4.3284622781876605
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.892823250296561,
+ "y": 5.546666666666668
+ },
+ "prevControl": {
+ "x": 6.892823250296561,
+ "y": 4.907866997335997
+ },
+ "nextControl": {
+ "x": 6.892823250296561,
+ "y": 6.2753696930420535
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.258030842230131,
+ "y": 6.977639383155397
+ },
+ "prevControl": {
+ "x": 6.423269381273585,
+ "y": 6.7900328128685725
+ },
+ "nextControl": {
+ "x": 6.092792303186677,
+ "y": 7.165245953442221
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 4.0,
+ "y": 7.5
+ },
+ "prevControl": {
+ "x": 5.5,
+ "y": 7.5
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1.2433392539964518,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 3.0373001776198882,
+ "rotationDegrees": 90.0
+ },
+ {
+ "waypointRelativePos": 4.129662522202486,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.9111361079864955,
+ "maxWaypointRelativePos": 2.125984251968506,
+ "constraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 2.0,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Stop Hood Down",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Stop Hood Down"
+ }
+ }
+ },
+ {
+ "name": "Start Spindexer",
+ "waypointRelativePos": 5.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 2.5,
+ "maxAngularVelocity": 200.0,
+ "maxAngularAcceleration": 300.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "reversed": false,
+ "folder": "week 5 (new stuff)",
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
},
"prevControl": {
"x": 8.155370348167532,
- "y": 0.8386737269319345
+ "y": 0.8386737269319344
},
"nextControl": {
"x": 8.45722898958081,
}
],
"pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "Stop Hood Down",
- "waypointRelativePos": 4.968503937007888,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Stop Hood Down"
- }
- }
- },
- {
- "name": "Start Spindexer",
- "waypointRelativePos": 5,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "Start Spindexer"
- }
- }
- }
- ],
+ "eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3,
"maxAcceleration": 2.5,
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.commands.LogCommand;
+import frc.robot.commands.auto_comm.DynamicAutoBuilder;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.gpm.AutoShootCommand;
import frc.robot.commands.gpm.ClimbDriveCommand;
}
}
+ public void addAuto(String name, Command auto) {
+ try {
+ autoChooser.addOption(name, auto);
+ }
+ // is this the right one??
+ catch (AutoBuilderException e) {
+ e.printStackTrace();
+ System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
+ }
+ }
+
/**
* Initialize the SendableChooser on the SmartDashboard.
* Fill the SendableChooser with available Commands.
String rightDoNothing = "Right Do Nothing";
String centerDoNothing = "Center Do Nothing";
+ String leftShallowDoubleSwipe = "LeftShallowDoubleSwipe";
+ String rightShallowDoubleSwipe = "RightShallowDoubleSwipe";
+
autoChooser.setDefaultOption("Default", getDefaultAuto());
addAuto(leftSideAuto);
addAuto(rightSideAuto);
addAuto(leftDoNothing);
addAuto(rightDoNothing);
addAuto(centerDoNothing);
+ addAuto(leftShallowDoubleSwipe);
+ addAuto(rightShallowDoubleSwipe);
+
+
+ DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, breaker);
+
+ String leftDynamicDoubleLiberalSwipe = "LeftDynamicDoubleLiberalSwipe";
+ String rightDynamicDoubleLiberalSwipe = "RightDynamicDoubleLiberalSwipe";
+ addAuto(leftDynamicDoubleLiberalSwipe, dynamicAutoBuilder.getLeftDynamicDoubleLiberalSwipe());
+ addAuto(rightDynamicDoubleLiberalSwipe, dynamicAutoBuilder.getRightDynamicDoubleLiberalSwipe());
// put the Chooser on the SmartDashboard
SmartDashboard.putData("Auto chooser", autoChooser);
--- /dev/null
+package frc.robot.commands.auto_comm;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.*;
+import frc.robot.commands.gpm.IntakeCommand;
+import frc.robot.commands.gpm.RunSpindexer;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.PowerControl.BreakerConstants;
+import frc.robot.subsystems.PowerControl.EMABreaker;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+public class DynamicAutoBuilder {
+
+ private final Spindexer spindexer;
+ private final Turret turret;
+ private final Hood hood;
+ private final Intake intake;
+ private final EMABreaker breaker;
+
+ public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake, EMABreaker breaker) {
+ this.spindexer = spindexer;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+ this.breaker = breaker;
+ }
+
+ public Command getLeftDynamicDoubleLiberalSwipe() {
+ return new SequentialCommandGroup(
+ new InstantCommand(() -> {
+ hood.forceHoodDown(true);
+ intake.extend();
+ intake.spinStart();
+ }),
+
+ new PathPlannerAuto("LeftSwipeOne"),
+
+ new InstantCommand(() -> hood.forceHoodDown(false)),
+ runSpindexerWithAbort(),
+ new InstantCommand(() -> hood.forceHoodDown(true)),
+
+ new PathPlannerAuto("LeftSwipeTwo"),
+ new InstantCommand(() -> hood.forceHoodDown(false)),
+ runSpindexerWithAbort(),
+ new InstantCommand(() -> hood.forceHoodDown(true)),
+
+ new PathPlannerAuto("LeftSwipeTwo"),
+ new InstantCommand(() -> hood.forceHoodDown(false)),
+ runSpindexerWithAbort(),
+ new InstantCommand(() -> hood.forceHoodDown(true)),
+
+ new PathPlannerAuto("LeftSwipeTwo"),
+ new InstantCommand(() -> hood.forceHoodDown(false)),
+ runSpindexerWithAbort(),
+ new InstantCommand(() -> hood.forceHoodDown(true))
+ );
+ }
+
+ public Command getRightDynamicDoubleLiberalSwipe() {
+ return new SequentialCommandGroup(
+ new InstantCommand(() -> hood.forceHoodDown(true)),
+
+ new PathPlannerAuto("RightSwipeOne"),
+ runSpindexerWithAbort(),
+
+ new PathPlannerAuto("RightSwipeTwo"),
+ runSpindexerWithAbort(),
+
+ new PathPlannerAuto("RightSwipeTwo"),
+ runSpindexerWithAbort(),
+
+ new PathPlannerAuto("RightSwipeTwo"),
+ runSpindexerWithAbort());
+ }
+
+ private Command runSpindexerWithAbort() {
+ // should race: when a command finnishes (will always be the wait until command)
+ // then we will end the command
+ // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new
+ // WaitCommand(5.0));
+
+ var timer = new Timer();
+ timer.start();
+
+ return new RunSpindexer(spindexer, turret, hood, intake).raceWith(
+ new WaitUntilCommand(() -> breaker.getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS) < 5.0 && timer.hasElapsed(3.0)));
+
+ // return new ParallelDeadlineGroup(new WaitUntilCommand(() ->
+ // spindexer.spinningAir()), new RunSpindexer(spindexer, turret, hood, intake));
+ // return new ParallelDeadlineGroup(new WaitCommand(5.0), new
+ // RunSpindexer(spindexer, turret, hood, intake));
+ }
+}
\ No newline at end of file
8.0); // Random initial goalState to prevent it being null
addRequirements(turret, shooter);
+
+ // SmartDashboard.putNumber("targetVelocity", 0.0);
}
public void updateSetpoints(Pose2d drivepose) {
turretOffset -= 2.5; //2.5 deg
}
+ // LoggedNetworkNumber velocity = new LoggedNetworkNumber("targetVelocity", 0.0);
+
@Override
public void execute() {
// Phase manager stuff
updateSetpoints(drivepose);
turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset);
- SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
+ // SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
if (phaseManager.isIdle()) {
underLadder();
// different maps for shuttling vs shooting. Less powerful when shuttling.
if (shuttling) {
shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget));
+ // shooter.setShooter(-SmartDashboard.getNumber("targetVelocity", 0.0));
} else {
shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget));
}
shooterVelocityMap.put(4.0, 12.8);
shooterVelocityMap.put(7.6, 19.0);
shooterVelocityMap.put(11.4, 28.1); // was 25.2 before
- shooterVelocityMap.put(16.54, 44.8); // untested
+ shooterVelocityMap.put(16.54, 70.0); // untested
// always shoot at low angle to ground.
newHoodMap.put(0.0, 60.0);
public double getAverageCurrentDraw(int[] ports) {
double sum = 0;
for (int number : ports) {
- sum += subsystems.get(number - 1).average; // the list starts at zero, so ports will be shifted
+ sum += subsystems.get(number).average; // the list starts at zero, so ports will be shifted
}
return sum;
}
);
CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT;
+ limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_STATOR_CURRENT_LIMIT;
limitConfig.StatorCurrentLimitEnable = true;
shooterMotorLeft.getConfigurator().apply(limitConfig);
shooterMotorRight.getConfigurator().apply(limitConfig);
SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0);
}
powerModifier = SmartDashboard.getNumber("OPERATOR: Shooter Power Modifier", powerModifier);
- SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier);
+ // SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier);
// keep this
- SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST");
+ Logger.recordOutput("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST");
}
/**
public static final double SHOOTER_VELOCITY = 1.0;
public static final double SHOOTER_LAUNCH_DIAMETER = Units.inchesToMeters(4.0);
public static final double SHOOTER_CURRENT_LIMIT = 40.0;
+ public static final double SHOOTER_STATOR_CURRENT_LIMIT = 120.0;
}