diff --git a/src/main/deploy/pathplanner/autos/center depot.auto b/src/main/deploy/pathplanner/autos/center depot.auto new file mode 100644 index 0000000..f1604d7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/center depot.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "backup" + } + }, + { + "type": "named", + "data": { + "name": "autoShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "depot pickup" + } + }, + { + "type": "named", + "data": { + "name": "autoIntake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "depot to hub " + } + }, + { + "type": "named", + "data": { + "name": "autoShoot" + } + } + ] + } + }, + "resetOdom": true, + "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 index bbd9194..660ca52 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,7 +1,7 @@ { "field_size": { - "x": 17.548, - "y": 8.052 + "x": 16.54, + "y": 8.07 }, "nodeSizeMeters": 0.3, "grid": [ @@ -61,9 +61,6 @@ true, true, true, - true, - true, - true, true ], [ @@ -122,15 +119,9 @@ true, true, true, - true, - true, - true, true ], [ - true, - true, - true, true, true, false, @@ -182,40 +173,15 @@ false, false, false, - true, - true, - true, + 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, @@ -226,6 +192,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -247,9 +220,6 @@ true, true, true, - true - ], - [ true, true, true, @@ -264,6 +234,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -274,6 +250,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -292,10 +275,12 @@ false, false, false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -323,6 +308,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -341,6 +333,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -350,24 +348,8 @@ 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 ], @@ -424,11 +406,8 @@ false, false, false, - false, - false, - false, - false, - false, + true, + true, true, true ], @@ -485,11 +464,8 @@ false, false, false, - false, - false, - false, - false, - false, + true, + true, true, true ], @@ -508,8 +484,6 @@ false, false, false, - true, - true, false, false, false, @@ -536,9 +510,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -552,14 +523,16 @@ false, false, true, + true, + true, true ], [ true, true, - false, - false, - false, + true, + true, + true, false, false, false, @@ -573,11 +546,7 @@ true, true, true, - false, - false, - false, - false, - false, + true, false, false, false, @@ -601,6 +570,7 @@ true, true, true, + true, false, false, false, @@ -611,17 +581,17 @@ false, false, false, - false, + true, true, true ], [ true, true, - false, - false, - false, - false, + true, + true, + true, + true, false, false, false, @@ -635,11 +605,7 @@ true, true, true, - false, - false, - false, - false, - false, + true, false, false, false, @@ -663,6 +629,7 @@ true, true, true, + true, false, false, false, @@ -679,9 +646,10 @@ [ true, true, - false, - false, - false, + true, + true, + true, + true, false, false, false, @@ -696,7 +664,6 @@ true, true, true, - true, false, false, false, @@ -705,9 +672,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -724,9 +688,6 @@ true, true, true, - true, - false, - false, false, false, false, @@ -735,9 +696,16 @@ false, false, true, + true, + true, + true, true ], [ + true, + true, + true, + true, true, true, false, @@ -745,10 +713,6 @@ false, false, false, - false, - false, - false, - true, true, true, true, @@ -765,11 +729,8 @@ false, false, false, - true, - true, - true, - true, - true, + false, + false, false, false, false, @@ -785,10 +746,6 @@ true, true, true, - true, - false, - false, - false, false, false, false, @@ -796,9 +753,17 @@ false, false, true, + true, + true, + true, + true, true ], [ + true, + true, + true, + true, true, true, false, @@ -806,10 +771,6 @@ false, false, false, - false, - false, - false, - true, true, true, true, @@ -826,11 +787,8 @@ false, false, false, - true, - true, - true, - true, - true, + false, + false, false, false, false, @@ -846,10 +804,6 @@ true, true, true, - true, - false, - false, - false, false, false, false, @@ -857,9 +811,17 @@ false, false, true, + true, + true, + true, + true, true ], [ + true, + true, + true, + true, true, true, false, @@ -867,10 +829,6 @@ false, false, false, - false, - false, - false, - true, true, true, true, @@ -887,11 +845,8 @@ false, false, false, - true, - true, - true, - true, - true, + false, + false, false, false, false, @@ -907,10 +862,6 @@ true, true, true, - true, - false, - false, - false, false, false, false, @@ -918,9 +869,16 @@ false, false, true, + true, + true, + true, + true, true ], [ + true, + true, + true, true, true, false, @@ -929,9 +887,6 @@ false, false, false, - false, - false, - true, true, true, true, @@ -949,9 +904,6 @@ false, false, false, - true, - true, - true, false, false, false, @@ -968,10 +920,6 @@ true, true, true, - true, - false, - false, - false, false, false, false, @@ -979,6 +927,10 @@ false, false, true, + true, + true, + true, + true, true ], [ @@ -1001,11 +953,7 @@ true, true, true, - false, - false, - false, - false, - false, + true, false, false, false, @@ -1029,16 +977,17 @@ true, true, true, + true, false, false, false, false, false, false, - false, - false, - false, - false, + true, + true, + true, + true, true, true ], @@ -1061,11 +1010,7 @@ true, true, true, - false, - false, - false, - false, - false, + true, false, false, false, @@ -1089,6 +1034,7 @@ true, true, true, + true, false, false, false, @@ -1097,13 +1043,14 @@ false, false, false, - false, - false, - false, + true, + true, + true, true, true ], [ + true, true, true, false, @@ -1118,8 +1065,6 @@ false, false, false, - true, - true, false, false, false, @@ -1146,9 +1091,7 @@ false, false, false, - true, - true, - true, + false, false, false, false, @@ -1167,10 +1110,7 @@ [ true, true, - false, - false, - false, - false, + true, false, false, false, @@ -1228,10 +1168,7 @@ [ true, true, - false, - false, - false, - false, + true, false, false, false, @@ -1287,6 +1224,7 @@ true ], [ + true, true, true, false, @@ -1298,6 +1236,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1316,23 +1261,12 @@ 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, @@ -1350,32 +1284,6 @@ [ 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, @@ -1386,6 +1294,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1406,9 +1321,6 @@ false, true, true, - true - ], - [ true, true, true, @@ -1424,6 +1336,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -1434,6 +1352,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1452,6 +1377,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1463,16 +1394,10 @@ false, false, false, - false, - true, - true, true, true ], [ - true, - true, - true, true, true, false, @@ -1524,9 +1449,9 @@ false, false, false, - true, - true, - true, + false, + false, + false, true, true ], @@ -1586,9 +1511,6 @@ true, true, true, - true, - true, - true, true ], [ @@ -1647,9 +1569,6 @@ true, true, true, - true, - true, - true, true ] ] diff --git a/src/main/deploy/pathplanner/paths/L1.path b/src/main/deploy/pathplanner/paths/L1.path index 6677534..1592c24 100644 --- a/src/main/deploy/pathplanner/paths/L1.path +++ b/src/main/deploy/pathplanner/paths/L1.path @@ -20,7 +20,7 @@ "y": 5.969204819277109 }, "prevControl": { - "x": 2.1223114027842978, + "x": 2.1223114027842973, "y": 6.161557495159928 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/backup.path b/src/main/deploy/pathplanner/paths/backup.path new file mode 100644 index 0000000..b72b78a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5732667617689007, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.8864779222010686, + "y": 3.997861470378837 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.35, + "y": 4.0 + }, + "prevControl": { + "x": 2.099960946645023, + "y": 4.002578483029655 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot pickup.path b/src/main/deploy/pathplanner/paths/depot pickup.path new file mode 100644 index 0000000..86c83cc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot pickup.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.35, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.316458321124934, + "y": 4.308092752867749 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5936519258202564, + "y": 5.98 + }, + "prevControl": { + "x": 2.0949953132832406, + "y": 5.98 + }, + "nextControl": { + "x": 1.101803135206326, + "y": 5.98 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7, + "y": 5.98 + }, + "prevControl": { + "x": 0.9915949012429583, + "y": 5.98 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.669243511871901, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot to hub .path b/src/main/deploy/pathplanner/paths/depot to hub .path new file mode 100644 index 0000000..354aff0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot to hub .path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 5.98 + }, + "prevControl": null, + "nextControl": { + "x": 1.7, + "y": 5.98 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7, + "y": 5.98 + }, + "prevControl": { + "x": 1.3347588374301795, + "y": 5.98 + }, + "nextControl": { + "x": 2.06524116256982, + "y": 5.98 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6416833095577745, + "y": 4.520199714693296 + }, + "prevControl": { + "x": 1.6416833095577745, + "y": 4.520199714693296 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..5f82a70 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.889, + "robotLength": 0.889, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.276, + "flModuleY": 0.276, + "frModuleX": 0.276, + "frModuleY": -0.276, + "blModuleX": -0.276, + "blModuleY": 0.276, + "brModuleX": -0.276, + "brModuleY": -0.276, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 30ae940..0210d36 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -139,7 +139,7 @@ public static final class ConductorConstants { } public static final class IndexerConstants { - public static final double RUN_VOLTAGE = 6; + public static final double RUN_VOLTAGE = 5; public static final double RUN_CURRENT = 30.0; public static final MotorConfig INDEXER_MOTOR_CONFIG = @@ -147,7 +147,7 @@ public static final class IndexerConstants { .withMotorInverted(true) .withSupplyCurrentLimit(40.0) .withStatorCurrentLimit(70.0) - .withIdleMode(MotorConfig.IdleMode.BRAKE); + .withIdleMode(MotorConfig.IdleMode.COAST); public static final DCMotor INDEXER_SIM_MOTOR = DCMotor.getKrakenX60(1); public static final DCMotorSim INDEXER_SIM = @@ -187,7 +187,7 @@ public static final class IntakeConstants { .withSupplyCurrentLimit(40.0) .withFeedbackConfig( FeedbackConfig.fused( - CANConstants.INTAKE_PIVOT_ENCODER_ID, PIVOT_GEAR_RATIO, 0.173, true)) + CANConstants.INTAKE_PIVOT_ENCODER_ID, PIVOT_GEAR_RATIO, 0.681, true)) .withProfileConstraintsRad( new TrapezoidProfile.Constraints( Units.degreesToRadians(1800), Units.degreesToRadians(540))); @@ -213,8 +213,8 @@ public static final class ShooterConstants { .withControlType(ControlType.VELOCITY) .withGearRatio(FLYWHEEL_GEAR_RATIO) .withMotorInverted(true) - .withSupplyCurrentLimit(40) - .withStatorCurrentLimit(70) + .withSupplyCurrentLimit(50) + .withStatorCurrentLimit(80) .withProfileConstraintsRad(new TrapezoidProfile.Constraints(1000, 1000)); public static final DCMotor FLYWHEEL_SIM_MOTOR = DCMotor.getKrakenX60(1); public static final DCMotorSim FLYWHEEL_SIM = @@ -235,7 +235,7 @@ public static final class KickerConstants { new MotorConfig() .withMotorInverted(true) .withSupplyCurrentLimit(30.0) - .withStatorCurrentLimit(40.0) + .withStatorCurrentLimit(60.0) .withIdleMode(MotorConfig.IdleMode.BRAKE); } diff --git a/src/main/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 6d9845e..3c542eb 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.MathShared; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.RobotController; @@ -160,7 +161,17 @@ public void robotPeriodic() { ExecutionLogger.log("Commands"); Logger.recordOutput("ShiftUtil/Official", HubShiftUtil.getOfficialShiftInfo()); + + Logger.recordOutput( + "ShiftUtil/Dashboard/CurrentShift", HubShiftUtil.getShiftedShiftInfo().currentShift()); + Logger.recordOutput( + "ShiftUtil/Dashboard/RemainingTime", HubShiftUtil.getShiftedShiftInfo().remainingTime()); + Logger.recordOutput("ShiftUtil/Dashboard/Active", HubShiftUtil.getShiftedShiftInfo().active()); + Logger.recordOutput("ShiftUtil/Shifted", HubShiftUtil.getShiftedShiftInfo()); + Logger.recordOutput("TurretManual", Units.radiansToDegrees(robotContainer.getTurretManual())); + Logger.recordOutput("FlywheelManual", robotContainer.getFlywheelManual()); + Logger.recordOutput("Vision/HasTags", robotContainer.getVision().hasTags()); robotContainer.updateAlerts(); FiringSolver.getInstance().clearCachedSolution(); diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 838c377..c100872 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -10,6 +10,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -102,6 +103,8 @@ public class RobotContainer { private final Trigger shiftAboutToEnd; private final Trigger activeOrPassing; + @Getter private double turretManual, flywheelManual; + public RobotContainer() { switch (Constants.CURRENT_MODE) { case REAL: @@ -151,7 +154,14 @@ public RobotContainer() { CANConstants.INTAKE_WHEEL_MOTOR_ID, IntakeConstants.INTAKE_WHEELS_MOTOR_CONFIG)); - conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + conductor = + new Conductor( + flywheel, + turret, + drive::getPose, + drive::getChassisSpeeds, + () -> turretManual, + () -> flywheelManual); leds = new LEDSubsystem( new LedIOCANdle(CANConstants.CANDLE_ID, 32), @@ -204,7 +214,14 @@ public RobotContainer() { turret = new Turret(new SmartMotorIO() {}); pivot = new Pivot(new SmartMotorIO() {}); - conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + conductor = + new Conductor( + flywheel, + turret, + drive::getPose, + drive::getChassisSpeeds, + () -> turretManual, + () -> flywheelManual); leds = new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); @@ -231,7 +248,14 @@ public RobotContainer() { turret = new Turret(new SmartMotorIO() {}); kicker = new Kicker(new DumbMotorIO() {}); - conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + conductor = + new Conductor( + flywheel, + turret, + drive::getPose, + drive::getChassisSpeeds, + () -> turretManual, + () -> flywheelManual); leds = new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); @@ -247,11 +271,13 @@ public RobotContainer() { conductor .runState(ConductorState.WARM_UP) .withTimeout(2.0) - .alongWith(pivot.holdAngle(0)) - .alongWith(wheels.in()) - .alongWith(indexer.in()) - .alongWith(kicker.in())); - + .andThen( + conductor + .runState(ConductorState.TRACKED_FIRING) + .alongWith(pivot.holdAngle(0)) + .alongWith(wheels.in()) + .alongWith(indexer.in()) + .alongWith(kicker.in()))); if (Constants.TUNING) setupDevelopmentRoutines(); SmartDashboard.putData( @@ -284,17 +310,26 @@ private void configureNamedCommands() { NamedCommands.registerCommand( "autoShoot", conductor - .runState(ConductorState.TRACKED_FIRING) - .alongWith(indexer.in()) - .alongWith(kicker.in()) - .withTimeout(2.0) - .finallyDo( - () -> { - indexer.stop(); - kicker.stop(); - })); + .runState(ConductorState.WARM_UP) + .withTimeout(1.0) + .andThen( + conductor + .runState(ConductorState.TRACKED_FIRING) + .alongWith(pivot.agitate()) + .alongWith(wheels.in()) + .alongWith(indexer.in()) + .alongWith(kicker.in()) + .withTimeout(4.0) + .finallyDo( + () -> { + wheels.stop().schedule(); + indexer.stop().schedule(); + kicker.stop().schedule(); + }))); - NamedCommands.registerCommand("autoIntake", wheels.in().finallyDo(() -> wheels.stop())); + NamedCommands.registerCommand( + "autoIntake", + wheels.in().alongWith(pivot.holdAngle(0)).finallyDo(() -> wheels.stop().schedule())); NamedCommands.registerCommand( "stopAll", @@ -357,7 +392,7 @@ private void configureBindings() { .onFalse(pivot.stop()); driverController - .povDown() + .povLeft() .whileTrue(pivot.agitate().alongWith(wheels.runIntake(5))) .onFalse(wheels.stop().alongWith(pivot.stop())); @@ -366,7 +401,8 @@ private void configureBindings() { .rightTrigger() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .and(activeOrPassing) - .whileTrue(Commands.parallel(indexer.in(), kicker.in())) + .whileTrue( + Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in()))) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Firing during inactive period @@ -379,8 +415,35 @@ private void configureBindings() { driverController .rightBumper() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) + .whileTrue( + Commands.waitSeconds(1).andThen(Commands.parallel(indexer.pulseIn(), kicker.in()))) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); + + // Operator override + operatorController + .rightTrigger() .whileTrue(Commands.parallel(indexer.in(), kicker.in())) .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); + operatorController + .rightBumper() + .whileTrue(Commands.parallel(indexer.out(), kicker.out(), wheels.out())) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), wheels.stop())); + + // Turret Zero + operatorController.back().onTrue(Commands.runOnce(() -> turret.zeroTurret())); + + // Manual Mode + operatorController + .start() + .toggleOnTrue(conductor.forceManual().alongWith(Commands.runOnce(this::resetManual))); + operatorController.povUp().whileTrue(Commands.run(() -> flywheelManual += 0.1)); + operatorController.povDown().whileTrue(Commands.run(() -> flywheelManual -= 0.1)); + operatorController + .povLeft() + .whileTrue(Commands.run(() -> turretManual -= Units.degreesToRadians(0.75))); + operatorController + .povRight() + .whileTrue(Commands.run(() -> turretManual += Units.degreesToRadians(0.75))); // Location Triggers allianceZoneTrigger @@ -438,6 +501,11 @@ public void updateAlerts() { operatorControllerAlert.set(!operatorController.isConnected()); } + public void resetManual() { + flywheelManual = flywheel.getVelocityMetersPerSec(); + turretManual = turret.getTurretPositionAsADouble(); + } + public static boolean withinBounds(double value, double bound1, double bound2) { return value <= Math.max(bound1, bound2) && value >= Math.min(bound1, bound2); } diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index cffb130..e7879be 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import lombok.experimental.Delegate; import org.team2342.frc.Constants; @@ -29,6 +30,7 @@ public class Conductor extends SubsystemBase { public enum ConductorState { UNDETERMINED, DISABLED, + MANUAL, WARM_UP, TRACKED_FIRING, TUNING, @@ -48,17 +50,25 @@ public enum ConductorState { private final Supplier poseSupplier; private final Supplier velocitySupplier; + private final DoubleSupplier manualTurretSupplier; + private final DoubleSupplier manualFlywheelSupplier; + public Conductor( Flywheel flywheel, Turret turret, Supplier poseSupplier, - Supplier velocitySupplier) { + Supplier velocitySupplier, + DoubleSupplier manualTurretSupplier, + DoubleSupplier manualFlywheelSupplier) { this.flywheel = flywheel; this.turret = turret; this.poseSupplier = poseSupplier; this.velocitySupplier = velocitySupplier; + this.manualFlywheelSupplier = manualFlywheelSupplier; + this.manualTurretSupplier = manualTurretSupplier; + setupStateCommands(); setupTransitions(); @@ -127,12 +137,23 @@ private void setupStateCommands() { .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) .alongWith(flywheel.shoot(flywheelSpeed))); + + fsm.addStateCommand( + ConductorState.MANUAL, + turret + .runPositionNoLimitCommand(manualTurretSupplier) + .alongWith(flywheel.shoot(manualFlywheelSupplier))); } public Command disable() { return Commands.runOnce(() -> fsm.disable()); } + public Command forceManual() { + return Commands.run(() -> fsm.forceState(ConductorState.MANUAL)) + .finallyDo(() -> fsm.forceState(ConductorState.DISABLED)); + } + public Command enable() { return Commands.runOnce(() -> fsm.enable()); } @@ -142,10 +163,11 @@ public ConductorState getCurrentState() { } private void setupTransitions() { - fsm.addOmniTransition(ConductorState.DISABLED); - fsm.addOmniTransition(ConductorState.TRACKED_FIRING); - fsm.addOmniTransition(ConductorState.WARM_UP); - fsm.addOmniTransition(ConductorState.TUNING); + fsm.addTransition(ConductorState.UNDETERMINED, ConductorState.DISABLED); + fsm.addDualTransition(ConductorState.DISABLED, ConductorState.TRACKED_FIRING); + fsm.addDualTransition(ConductorState.WARM_UP, ConductorState.TRACKED_FIRING); + fsm.addDualTransition(ConductorState.DISABLED, ConductorState.WARM_UP); + fsm.addDualTransition(ConductorState.DISABLED, ConductorState.TUNING); } public interface FSMDelegate { diff --git a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java index 36edbf4..697f4f0 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java @@ -51,7 +51,7 @@ public Command out() { public Command pulseIn() { return Commands.repeatingSequence( - in().withTimeout(0.25), stop().andThen(Commands.waitSeconds(0.25))); + in().withTimeout(2), stop().andThen(Commands.waitSeconds(0.5))); } public Command stop() { diff --git a/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java index a7f09f6..d2323c1 100644 --- a/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java +++ b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java @@ -25,14 +25,14 @@ public class Pivot extends SubsystemBase { private final SmartMotorIOInputsAutoLogged pivotMotorInputs = new SmartMotorIOInputsAutoLogged(); @AutoLogOutput(key = "Intake/Pivot/TargetAngle") - private double goal = IntakeConstants.MIN_ANGLE; + private double goal = IntakeConstants.MAX_ANGLE; private final Alert pivotMotorAlert = new Alert("Pivot motor is disconnected!", AlertType.kError); public Pivot(SmartMotorIO pivotMotor) { this.pivotMotor = pivotMotor; setName("Intake/Pivot"); - setDefaultCommand(holdAngle(goal)); + setDefaultCommand(run(() -> pivotMotor.runPosition(goal))); } @Override @@ -71,6 +71,6 @@ public Command stop() { } public Command agitate() { - return Commands.repeatingSequence(goToAngle(0.8), goToAngle(0.9)); + return Commands.repeatingSequence(goToAngle(1.4), goToAngle(1.5)); } } diff --git a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java index 90cbd21..b2b73d9 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -53,10 +54,19 @@ public void runPosition(Supplier target) { turretMotor.runPosition(goal); } + public void runPositionNoLimit(DoubleSupplier target) { + this.goal = target.getAsDouble(); + turretMotor.runPosition(goal); + } + public Command runPositionCommand(Rotation2d target) { return run(() -> runPosition(target)).withName("Turret RunPosition"); } + public Command runPositionNoLimitCommand(DoubleSupplier target) { + return run(() -> runPositionNoLimit(target)).withName("Turret RunPosition No Limit"); + } + public Command runPositionCommand(Supplier target) { return run(() -> runPosition(target)).withName("Turret RunPosition"); } @@ -86,6 +96,10 @@ public Rotation2d getTurretPosition() { return Rotation2d.fromRadians(inputs.positionRad); } + public double getTurretPositionAsADouble() { + return inputs.positionRad; + } + public double getTurretVelocity() { return inputs.velocityRadPerSec; } @@ -100,7 +114,8 @@ public boolean atGoal() { } public void zeroTurret() { - turretMotor.setPosition(0.0); + turretMotor.setPosition(TurretConstants.STARTING_ANGLE); + goal = TurretConstants.STARTING_ANGLE; } private double calculateTurretAngle(Rotation2d angle) { diff --git a/src/main/java/org/team2342/frc/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index 50f8c7f..aa9cbaa 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -32,16 +32,16 @@ public class FiringSolver { // TODO: tune real maps static { speedMap.put(1.942, 14.2); - speedMap.put(2.712, 14.2); - speedMap.put(2.847, 15.0); - speedMap.put(3.065, 16.0); - speedMap.put(3.442, 17.0); - speedMap.put(3.8, 17.5); - speedMap.put(4.16, 19.0); - speedMap.put(4.341, 20.5); + speedMap.put(2.712, 14.7); + speedMap.put(2.847, 15.5); + speedMap.put(3.065, 17.5); + speedMap.put(3.442, 18.0); + speedMap.put(3.8, 18.5); + speedMap.put(4.16, 20.5); + speedMap.put(4.341, 21.5); MIN_TOF = 3.89 - 3.08; - MAX_TOF = 1.0; + MAX_TOF = 11.3 - 9.83; tofMap.put(1.942, 3.89 - 3.08); tofMap.put(2.712, 4.11 - 3.30);