diff --git a/lib/RoboLib b/lib/RoboLib index cd9dee7..0584a07 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12 +Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51 diff --git a/simgui-ds.json b/simgui-ds.json index e3a2a39..512b0fe 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -7,17 +12,20 @@ "incKey": 68 }, { - "decKey": 87, - "incKey": 83 + "decKey": 83, + "incKey": 87 }, { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, + "decayRate": 0.009999999776482582, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -91,7 +99,7 @@ ], "robotJoysticks": [ { - "guid": "030000004c050000cc09000011810000" + "guid": "Keyboard0" } ] } diff --git a/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto b/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto index a7380ff..216b0a1 100644 --- a/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto +++ b/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto @@ -9,6 +9,25 @@ "data": { "pathName": "S. Hub. shoot 8. E-Hub" } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, + { + "type": "named", + "data": { + "name": "ChassisAlignShoot" + } + } + ] + } } ] } diff --git a/src/main/deploy/pathplanner/paths/M. Depot. .path b/src/main/deploy/pathplanner/paths/M. Depot. .path index 990a3af..2e111ae 100644 --- a/src/main/deploy/pathplanner/paths/M. Depot. .path +++ b/src/main/deploy/pathplanner/paths/M. Depot. .path @@ -36,7 +36,7 @@ "y": 7.138469879518072 }, "prevControl": { - "x": 0.4124120733073493, + "x": 0.41241207330734936, "y": 6.876706525398199 }, "nextControl": { diff --git a/src/main/deploy/pathplanner/paths/Relay. R-Tr..path b/src/main/deploy/pathplanner/paths/Relay. R-Tr..path index 1edcd79..abea361 100644 --- a/src/main/deploy/pathplanner/paths/Relay. R-Tr..path +++ b/src/main/deploy/pathplanner/paths/Relay. R-Tr..path @@ -37,7 +37,7 @@ }, "prevControl": { "x": 7.691609840125085, - "y": 7.387443181134998 + "y": 7.387443181134999 }, "nextControl": { "x": 7.64634196710383, @@ -56,7 +56,7 @@ "y": 7.138469879518072 }, "nextControl": { - "x": 6.511975903614461, + "x": 6.511975903614458, "y": 7.138469879518072 }, "isLocked": false, @@ -85,11 +85,11 @@ }, "prevControl": { "x": 6.020824888067715, - "y": 0.8531028227756468 + "y": 0.8531028227756466 }, "nextControl": { "x": 5.667271497474452, - "y": 1.2066562133689314 + "y": 1.2066562133689316 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path b/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path index 8793d07..87aa838 100644 --- a/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path +++ b/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.473, - "y": 4.013 + "x": 2.9483700538099855, + "y": 4.00221686746988 }, "prevControl": { - "x": 3.1233132530120478, - "y": 4.034855421686746 + "x": 2.5986833068220334, + "y": 4.024072289156626 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path b/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path index 8fdebe3..8b6f670 100644 --- a/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path +++ b/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path @@ -24,7 +24,7 @@ "y": 3.77441480327739 }, "nextControl": { - "x": 2.348744281004639, + "x": 2.3487442810046395, "y": 4.0114647147949 }, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 39b2ea8..d0cccb7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,12 @@ package frc.robot; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; @@ -76,6 +82,19 @@ public final class Constants { public static final int BACK_RIGHT_DRIVE_ID = 42; public static final int BACK_RIGHT_ENCODER_ID = 43; } + + public static class RobotChassisLimits{ + // Max velocity in meters per second + public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; + // In meters per second squared + public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; + + // In radians per second + public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; + // In radians per second squared + public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; + } + //Bird public static class ExtraIDEntities { public static final int PIGEON_ID = 1; @@ -121,4 +140,12 @@ public final class Constants { public static final int SPINDEXER_MOTOR_ID = 8; public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; } + + public static class FieldPoses { + private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f79e09..9d8bb69 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -11,10 +13,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutoShootCommandChassisTurret; +import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.ManualHopperCommand; import frc.robot.commands.ManualIntake; import frc.robot.commands.ManualShootCommand; +import frc.robot.commands.RelayCommand; import frc.robot.commands.SetDeflectorCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -45,11 +49,14 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); // turretSubsystem.homeTurret(); // Configure the trigger bindings configureBindings(); + + NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + } /** @@ -68,8 +75,14 @@ public class RobotContainer { m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); + m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + // m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + + m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); - m_driverController.y().onTrue(intakeSubsystem.deploy()); + m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); + m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } @@ -80,6 +93,6 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Commands.parallel(Commands.run(()-> {/*turretSubsystem.homeTurret();*/}, turretSubsystem), driveSubsystem.getAutoCommand()); + return Commands.parallel(intakeSubsystem.deploy(), Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem), driveSubsystem.getAutoCommand()); } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index d7ca341..a524d0e 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -75,6 +75,7 @@ public class AutoShootCommandChassisTurret extends Command{ }); turretSubsystem.setLauncher(5500); + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); hopperSubsystem.spindexerOn(); cameraAutoCenterController.calculate(-100); @@ -91,8 +92,12 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - // Add 180 because we want the back of the robot to face the HUB - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + // Add 180 because we want the back of the robot to face the HUB + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); + } + }); } @Override diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java new file mode 100644 index 0000000..93238d5 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java @@ -0,0 +1,130 @@ +package frc.robot.commands; + +import java.util.Arrays; +import java.util.List; + +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; +import org.littletonrobotics.junction.Logger; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class AutoShootCommandChassisTurretPathPlanner extends Command { + + // For Chassis Control + private DriveSubsystem driveSubsystem; + + private HopperSubsystem hopperSubsystem; + private TurretSubsystem turretSubsystem; + + private int[] tagFilters; + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + private Pose3d closestTag; + private Pose2d targetPPPose; + + private Command pathFollowCommand; + + public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, + TurretSubsystem turretSubsystem) { + this.driveSubsystem = driveSubsystem; + this.hopperSubsystem = hopperSubsystem; + this.turretSubsystem = turretSubsystem; + + this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); + + } + + @Override + public void initialize() { + + DriverStation.getAlliance().ifPresent((aliance) -> { + Pose3d targetHub; + if (aliance == Alliance.Blue) { + tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; + targetHub = Constants.FieldPoses.BLUE_HUB_POSE; + } else { + tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; + targetHub = Constants.FieldPoses.RED_HUB_POSE; + } + + Pose3d botPose = driveSubsystem.getBotPose(); + // Find closest target to us + List tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()) + .toList(); + closestTag = botPose.nearest(tagPoses); + Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag); + + Translation2d delta = botPose.getTranslation().toTranslation2d() + .minus(closestTag.getTranslation().toTranslation2d()); + Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta); + + // Add 180 because we want the back of the robot to face the HUB + // deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); + + // Translate back 1 meter from HUB + Translation2d newTrans = targetHub.getTranslation().toTranslation2d() + .plus(new Translation2d(1.5, delta.getAngle())); + targetPPPose = new Pose2d(newTrans, delta.getAngle()); + Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose); + + pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose, + new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY, + Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL, + Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY, + Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL)); + + CommandScheduler.getInstance().schedule(pathFollowCommand); + }); + + turretSubsystem.setLauncher(5500); + hopperSubsystem.spindexerOn(); + + } + + @Override + public void execute() { + + // If robot is centered on HUB and shooter is up to speed. Run the kicker + if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) { + Logger.recordOutput("AutoAlignState-Shoot", "FIRE!"); + hopperSubsystem.feedKicker(); + } else { + Logger.recordOutput("AutoAlignState-Shoot", "Waiting"); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + pathFollowCommand.cancel(); + turretSubsystem.setLauncher(0); + + hopperSubsystem.stopKicker(); + hopperSubsystem.spindexerOff(); + + driveSubsystem.stopDrive(); + + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, + Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER); + } + +} diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index da975ef..16455a0 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -29,8 +29,6 @@ public class AutoTurretCommand extends Command { private int[] shootingTagFilters; - private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); - private Pose3d closestTag; private Pose3d scoringHubPose; @@ -49,14 +47,10 @@ public class AutoTurretCommand extends Command { DriverStation.getAlliance().ifPresent((aliance) -> { if (aliance == Alliance.Blue) { shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; - - scoringHubPose = fieldTags.getTagPose(26).get(); - scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE; } else { shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; - - scoringHubPose = fieldTags.getTagPose(10).get(); - scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + scoringHubPose = Constants.FieldPoses.RED_HUB_POSE; } Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose); }); @@ -76,7 +70,7 @@ public class AutoTurretCommand extends Command { if(turretSubsystem.isTurretReady()){ - turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); + turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees)); } diff --git a/src/main/java/frc/robot/commands/RelayCommand.java b/src/main/java/frc/robot/commands/RelayCommand.java new file mode 100644 index 0000000..754f9d7 --- /dev/null +++ b/src/main/java/frc/robot/commands/RelayCommand.java @@ -0,0 +1,59 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class RelayCommand extends Command{ + + private HopperSubsystem hopperSubsystem; + private TurretSubsystem turretSubsystem; + private IntakeSubsystem intakeSubsystem; + + private Command waitCommand = Commands.waitSeconds(1.5); + + public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){ + this.hopperSubsystem = hopperSubsystem; + this.turretSubsystem = turretSubsystem; + this.intakeSubsystem = intakeSubsystem; + + addRequirements(hopperSubsystem, turretSubsystem, intakeSubsystem); + } + + @Override + public void initialize() { + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3); + turretSubsystem.setLauncher(5500); + hopperSubsystem.spindexerOn(); + CommandScheduler.getInstance().schedule(waitCommand); + } + + @Override + public void execute() { + if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){ + hopperSubsystem.feedKicker(); + }else{ + hopperSubsystem.stopKicker(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + waitCommand.cancel(); + hopperSubsystem.spindexerOff(); + hopperSubsystem.stopKicker(); + + turretSubsystem.setLauncher(0); + turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + } + +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 6dbad45..d5066ca 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -10,6 +10,8 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -50,8 +52,8 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem = swerveBuilder .withCanbusName("Drive") .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID) - .withMaxLinearVelocity(3.0) - .withMaxAngularVelocity(Math.PI) + .withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY) + .withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY) .withSwerveModule( swerveBuilder.new SwerveModuleBuilder() .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID) @@ -135,7 +137,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); this.setDefaultCommand(Commands.run(new Runnable() { @@ -163,8 +165,8 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem::getRobotRelativeSpeeds, (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()), new PPHolonomicDriveController( - new PIDConstants(5.0, 0, 0), - new PIDConstants(5.0, 0, 0)), + new PIDConstants(1.5, 0, 0), + new PIDConstants(Math.PI/2, 0, 0)), config, () -> { var alliance = DriverStation.getAlliance(); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1b8ebde..7d0e930 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -41,21 +41,18 @@ public class IntakeSubsystem extends TestableSubsystem { return Commands.sequence( Commands.deadline( Commands.waitSeconds(2), - this.runEnd( - () -> {deployMotor.set(0.5);}, // Bump this speed up if intake doesn't deploy - () -> {deployMotor.stopMotor();} + this.runOnce( + () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy ) ), - Commands.deadline( - Commands.waitSeconds(2), - this.runEnd( - () -> { deployMotor.set(-0.5); }, // Same here - () -> { deployMotor.stopMotor(); } - ) - ) + this.runOnce(() -> deployMotor.stopMotor()) ); } + public Command resetDeploy(){ + return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); + } + public void setSpeed(double speed) { intakeMotor.set(speed); @@ -63,7 +60,7 @@ public class IntakeSubsystem extends TestableSubsystem { public Command intake() { - return this.runOnce(() -> {if(intakeMotor.get()!=0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); + return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); } public Command reverseIntake() diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6dbab83..99a289f 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem { new TalonFXConfiguration() .withSlot0( new Slot0Configs() - .withKP(2) + .withKP(3) .withKI(1) .withKD(0) - .withKS(0) + .withKS(0.3) ) .withMotorOutput( new MotorOutputConfigs() @@ -134,11 +134,6 @@ public class TurretSubsystem extends TestableSubsystem { return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1; } - public void turretControl(double position) - { - turretMotor.setControl(new PositionVoltage(position)); - } - public void setTargetElevation(double elev) { double pos = Math.abs((elev - 300)/300); @@ -161,6 +156,9 @@ public class TurretSubsystem extends TestableSubsystem { public void homeTurret() { + if(turretState != TurretState.INIT){ + return; // Early return, we don't want to home if we already have + } turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); turretState = TurretState.HOMING; } @@ -193,7 +191,7 @@ public class TurretSubsystem extends TestableSubsystem { if(turretLimitSwitch.get()) { // Turret has tripped the switch turretMotor.stopMotor(); - turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); + turretOffset = turretMotorPosition.refresh().getValue().minus(Angle.ofRelativeUnits(65, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); turretState = TurretState.READY; Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset); @@ -201,12 +199,13 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); turretMotor.getConfigurator().apply( new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) + // .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(forwardLimit) - .withReverseSoftLimitEnable(true) + // .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(reverseLimit) ); - //setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees)); + // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); + setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees)); } } @@ -226,7 +225,16 @@ public class TurretSubsystem extends TestableSubsystem { } - public void setTurretRotation(Angle pos) + public void setTurretFieldRelative(Angle pos) + { + if(turretState != TurretState.READY) + { + // Not ready to set angle + return; + } + } + + public void setTurretRobotRelative(Angle pos) { if(turretState != TurretState.READY) { @@ -237,8 +245,8 @@ public class TurretSubsystem extends TestableSubsystem { setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } - public void setTurretMotorRotation(Angle pos){ - turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset))); + private void setTurretMotorRotation(Angle pos){ + turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1))); turretSim.setRawRotorPosition(pos.plus(turretOffset)); }