diff --git a/src/main/deploy/logs/.gitkeep b/src/main/deploy/logs/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 00000000..91491b7b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.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 index a8ea08de..f3cb5fdc 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -18,7 +18,7 @@ "driveGearing": 6.12, "maxDriveSpeed": 4.4, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 100.0, + "driveCurrentLimit": 120.0, "wheelCOF": 1.6, "flModuleX": 0.172, "flModuleY": 0.273, diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index b54374ce..6a8a1ece 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -13,10 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.*; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.LEDConstants; -import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.*; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; @@ -24,13 +21,10 @@ import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.armelevator.ArmElevator; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; -import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; -import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.endeffector.EndEffector; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; -import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -45,7 +39,7 @@ public class RobotContainer { public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator( - CameraConstants.INTAKE_SIDE_REEF_TAG_CAMERA, + CameraConstants.FRONT_REEF_TAG_CAMERA, CameraConstants.LEFT_REEF_TAG_CAMERA, CameraConstants.RIGHT_REEF_TAG_CAMERA ); @@ -80,13 +74,14 @@ public Command getAutonomousCommand() { private void configureBindings() { bindDefaultCommands(); bindControllerCommands(); +// configureSysIDBindings(ARM_ELEVATOR); } private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand()); - CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); - END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST)); + CLIMBER.setDefaultCommand(ClimberCommands.getDefaultCommand()); + END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getDefaultCommand()); INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); } @@ -118,7 +113,15 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))); OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); + OperatorConstants.SHOULD_LOAD_CORAL_TOGGLE_TRIGGER.onTrue(GeneralCommands.getToggleShouldLoadCoralCommand()); + OperatorConstants.SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldManipulateCoralAutonomouslyCommand()); + OperatorConstants.SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldCollectCoralAtonomouslyCommand()); + OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand()); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); +// OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ClimberCommands.getDebuggingCommand()); + OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.whileTrue(ClimberCommands.resetClimberPositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.whileTrue(IntakeCommands.resetIntakePositionCommand().ignoringDisable(true)); + OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.whileTrue(ArmElevatorCommands.resetElevatorPositionCommand().ignoringDisable(true)); } private void configureSysIDBindings(MotorSubsystem subsystem) { @@ -161,7 +164,11 @@ private void buildAutoChooser() { addCommandsToChooser( AutonomousCommands.getFloorAutonomousCommand(true), - AutonomousCommands.getFloorAutonomousCommand(false) + AutonomousCommands.getFloorAutonomousCommand(false), + AutonomousCommands.getFloorAutonomousCommand(true, FieldConstants.ReefClockPosition.REEF_2_OCLOCK, FieldConstants.ReefClockPosition.REEF_4_OCLOCK), + AutonomousCommands.getFloorAutonomousCommand(false, FieldConstants.ReefClockPosition.REEF_8_OCLOCK, FieldConstants.ReefClockPosition.REEF_10_OCLOCK), + AutonomousCommands.getFloorAutonomousCommand(true, FieldConstants.ReefClockPosition.REEF_2_OCLOCK, FieldConstants.ReefClockPosition.REEF_4_OCLOCK, FieldConstants.ReefClockPosition.REEF_6_OCLOCK), + AutonomousCommands.getFloorAutonomousCommand(false, FieldConstants.ReefClockPosition.REEF_8_OCLOCK, FieldConstants.ReefClockPosition.REEF_10_OCLOCK, FieldConstants.ReefClockPosition.REEF_6_OCLOCK) ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index ba2d2cec..e1264b71 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -23,13 +23,13 @@ public class IntakeAssistCommand extends ParallelCommandGroup { static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.7, 0, 0, new TrapezoidProfile.Constraints(2.65, 4.5)), Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), + new ProfiledPIDController(0.25, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.8, 4.5)); private Translation2d distanceFromTrackedGamePiece; /** @@ -96,8 +96,8 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getX())); final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getY())); - if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) - return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + if (assistMode.isAlternate) + return calculateAlternateAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput, intakeAssistScalar); } @@ -115,14 +115,13 @@ private static double calculateThetaPower(AssistMode assistMode, Translation2d d return pow; } - private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { - final double pidScalar = Math.cbrt(joystickValue.getNorm()); + private static Translation2d calculateAlternateAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { final double - xJoystickPower = Math.cbrt(joystickValue.getX()), - yJoystickPower = Math.cbrt(joystickValue.getY()); + xJoystickPower = joystickValue.getX(), + yJoystickPower = joystickValue.getY(); final double - xPower = calculateAlternateAssistPower(xPIDOutput, pidScalar, xJoystickPower), - yPower = calculateAlternateAssistPower(yPIDOutput, pidScalar, yJoystickPower); + xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower; return new Translation2d(xPower, yPower); } @@ -143,8 +142,8 @@ private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2 pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); - if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) - return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue); + if (assistMode.isAlternate) + return calculateAlternateAssistPower(pidOutput, joystickValue); return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar); } @@ -152,8 +151,8 @@ private static double clampToOutputRange(double value) { return MathUtil.clamp(value, -1, 1); } - private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { - return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower; + private static double calculateAlternateAssistPower(double pidOutput, double joystickPower) { + return pidOutput * (1 - Math.abs(joystickPower)) + joystickPower; } private static double calculateNormalAssistPower(double pidOutput, double joystickPower, double scalar) { @@ -162,6 +161,7 @@ private static double calculateNormalAssistPower(double pidOutput, double joysti private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) { X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond); + X_PID_CONTROLLER.setGoal(-0.15); Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond); THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); } @@ -173,29 +173,35 @@ public enum AssistMode { /** * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. */ - ALTERNATE_ASSIST(true, true, true), + ALTERNATE_ASSIST(true, true, true, true), + /** + * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. + */ + ALTERNATE_ALIGN(false, true, true, true), /** * Applies pid values to autonomously drive to the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ - FULL_ASSIST(true, true, true), + FULL_ASSIST(true, true, true, false), /** * Applies pid values to align to the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ - ALIGN_ASSIST(false, true, true), + ALIGN_ASSIST(false, true, true, false), /** * Applies pid values to face the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ - ASSIST_ROTATION(false, false, true); + ASSIST_ROTATION(false, false, true, false); final boolean shouldAssistX, shouldAssistY, - shouldAssistTheta; + shouldAssistTheta, + isAlternate; - AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) { + AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta, boolean isAlternate) { this.shouldAssistX = shouldAssistX; this.shouldAssistY = shouldAssistY; this.shouldAssistTheta = shouldAssistTheta; + this.isAlternate = isAlternate; } } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 41268ba7..80ab0ad5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))), new InstantCommand(() -> { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; @@ -64,7 +64,7 @@ public static Command getReefAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateReefAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))), new InstantCommand(() -> IS_HOLDING_ALGAE = true), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) @@ -110,6 +110,13 @@ private static Command getHoldAlgaeCommand() { } private static Command getScoreInNetCommand() { + return new ParallelRaceGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER) + ); + } + + private static Command getAutonomouslyScoreInNetCommand() { return new ParallelRaceGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), @@ -119,7 +126,14 @@ private static Command getScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, false), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER) + ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); + } + + private static Command getAtonomouslyScoreInProcessorCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR), GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), getDriveToProcessorCommand() ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); @@ -181,7 +195,7 @@ private static Command getCollectAlgaeFromLollipopSequenceCommand() { .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) .repeatedly() - ); + ).finallyDo(() -> SHOULD_COLLECT_FROM_LOLLIPOP = false); } private static Command getCollectAlgaeFromFloorSequenceCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index bef54960..96157c35 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -2,6 +2,8 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; @@ -32,13 +34,13 @@ public class AutonomousCommands { private static FlippablePose2d TARGET_SCORING_POSE = null; private static boolean IS_FIRST_CORAL = true; - public static Command getFloorAutonomousCommand(boolean isRight) { - return getCycleCoralCommand(isRight).repeatedly().withName("FloorAutonomous" + (isRight ? "Right" : "Left")); + public static Command getFloorAutonomousCommand(boolean isRight, FieldConstants.ReefClockPosition... reefClockPositions) { + return getCycleCoralCommand(isRight, reefClockPositions).repeatedly().withName("FloorAutonomous" + (isRight ? "Right" : "Left") + (reefClockPositions.length * 2) + "Branches"); } - public static Command getCycleCoralCommand(boolean isRight) { + public static Command getCycleCoralCommand(boolean isRight, FieldConstants.ReefClockPosition[] reefClockPositions) { return new SequentialCommandGroup( - getDriveToReefAndScoreCommand(), + getDriveToReefAndScoreCommand(reefClockPositions), getCollectCoralCommand(isRight) ); } @@ -49,7 +51,7 @@ public static Command getFindCoralCommand(boolean isRight) { SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, - () -> AutonomousConstants.AUTO_FIND_CORAL_ROTATION_POWER + () -> 0 ) ); } @@ -81,31 +83,32 @@ private static Command getDriveToFirstCoralPoseCommand(boolean isRight) { ).alongWith(new InstantCommand(() -> IS_FIRST_CORAL = false)); } - public static Command getDriveToReefAndScoreCommand() { + public static Command getDriveToReefAndScoreCommand(FieldConstants.ReefClockPosition[] reefClockPositions) { return new ParallelRaceGroup( - getDriveToReefCommand(), + getDriveToReefCommand(reefClockPositions), getCoralSequenceCommand() ); } - public static Command getCollectCoralCommand(boolean isRight) { - return new ParallelCommandGroup( - CoralCollectionCommands.getIntakeSequenceCommand(), - ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), - getDriveToCoralCommand(isRight) - ) - .until(RobotContainer.INTAKE::hasCoral) - .unless(() -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.END_EFFECTOR.hasGamePiece()); - } - - public static Command getDriveToReefCommand() { + public static Command getDriveToReefCommand(FieldConstants.ReefClockPosition[] reefClockPositions) { return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), + new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose(reefClockPositions, false)), new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> calculateClosestOpenScoringPose(reefClockPositions, true), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() ); } + public static Command getCollectCoralCommand(boolean isRight) { + return new ParallelCommandGroup( + CoralCollectionCommands.getIntakeCoralCommand(), + ArmElevatorCommands.getSetTargetStateCommand(() -> ArmElevatorConstants.ArmElevatorState.REST), + getDriveToCoralCommand(isRight) + ) + .until(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.TRANSPORTER.hasCoral()) + .unless(RobotContainer.TRANSPORTER::hasCoral); + } + public static Command getCoralSequenceCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getLoadCoralCommand(), @@ -114,14 +117,6 @@ public static Command getCoralSequenceCommand() { ); } - public static Command getDriveToCoralCommand(boolean isRight) { - return new ConditionalCommand( - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(10), - getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null - ).repeatedly(); - } - public static Command getScoreCommand() { return new SequentialCommandGroup( getOpenElevatorWhenCloseToReefCommand().until(AutonomousCommands::canScore), @@ -157,13 +152,13 @@ private static double calculateDistanceToTargetScoringPose() { return currentTranslation.getDistance(targetTranslation); } - public static FlippablePose2d calculateClosestOpenScoringPose() { + public static FlippablePose2d calculateClosestOpenScoringPose(FieldConstants.ReefClockPosition[] reefClockPositions, boolean shouldStayBehindAlgae) { final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); double closestDistance = Double.POSITIVE_INFINITY; Pose2d closestScoringPose = null; - for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { + for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { if (scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) continue; @@ -171,7 +166,10 @@ public static FlippablePose2d calculateClosestOpenScoringPose() { final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); if (distance < closestDistance) { closestDistance = distance; - closestScoringPose = reefSideScoringPose; + if (shouldStayBehindAlgae) + closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(FieldConstants.REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS - FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS, 0), new Rotation2d())); + else + closestScoringPose = reefSideScoringPose; } } } @@ -179,6 +177,14 @@ public static FlippablePose2d calculateClosestOpenScoringPose() { return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); } + public static Command getDriveToCoralCommand(boolean isRight) { + return new ConditionalCommand( + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(10), + getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null + ).repeatedly(); + } + private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { return new InstantCommand( () -> { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index ff9cd032..9c2a8584 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -15,13 +15,15 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; public class ClimbCommands { - private static boolean IS_CLIMBING = false;//TODO: Make score triggers not work while climbing + private static boolean IS_CLIMBING = false; public static Command getClimbCommand() { return new SequentialCommandGroup( new InstantCommand(() -> IS_CLIMBING = true), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.BREAK_ZIP_TIE) + .until(() -> RobotContainer.CLIMBER.atState(ClimberConstants.ClimberState.BREAK_ZIP_TIE)), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB) - .until(() -> RobotContainer.CLIMBER.hasCage() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()), + .until(OperatorConstants.CONTINUE_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) .until(RobotContainer.CLIMBER::atTargetState), getAdjustClimbManuallyCommand() @@ -36,19 +38,19 @@ public static boolean isClimbing() { private static Command getAdjustClimbManuallyCommand() { return new ParallelCommandGroup( - ClimberCommands.getSetTargetSpeedCommand(OperatorConstants.DRIVER_CONTROLLER::getRightY), + ClimberCommands.getSetTargetSpeedCommand(() -> OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, () -> 0 - ) + ).asProxy() ); } private static Command getSetSubsystemsToRestForClimbCommand() { return new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_FOR_CLIMB), - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST_FOR_CLIMB) + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST) ); } } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 14f12b00..7fb8f1e3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -15,25 +15,46 @@ import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; import frc.trigon.robot.subsystems.transporter.TransporterConstants; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; public class CoralCollectionCommands { + public static boolean SHOULD_LOAD_CORAL = true; + public static boolean SHOULD_USE_INTAKE_ASSIST = false; + public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( - getIntakeSequenceCommand(), + getIntakeCoralCommand().until(RobotContainer.TRANSPORTER::hasCoral).unless((RobotContainer.TRANSPORTER::hasCoral)), + getCollectionConfirmationCommand(), new InstantCommand( () -> { - if (!AlgaeManipulationCommands.isHoldingAlgae()) + if (!AlgaeManipulationCommands.isHoldingAlgae() && SHOULD_LOAD_CORAL) getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)).unless(CoralCollectionCommands::hasCoral); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).until(RobotContainer.INTAKE::hasCoral).asProxy().onlyIf(() -> CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST) + ).finallyDo(() -> SHOULD_LOAD_CORAL = true); } public static Command getLoadCoralCommand() { - return new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), - EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) - ).until(RobotContainer.END_EFFECTOR::hasGamePiece); + return new SequentialCommandGroup( + new ParallelCommandGroup( + getIntakeCoralCommand(), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST) + ).until(RobotContainer.TRANSPORTER::hasCoral).unless(RobotContainer.TRANSPORTER::hasCoral).unless(RobotContainer.END_EFFECTOR::hasGamePiece).finallyDo(() -> {RobotContainer.TRANSPORTER.stop(); RobotContainer.INTAKE.setTargetState(IntakeConstants.IntakeState.REST);}), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST).unless(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST)), + new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) + ).until(RobotContainer.END_EFFECTOR::hasGamePiece), + new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL) + ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST_AFTER_LOADING)) + ).unless(() -> RobotContainer.END_EFFECTOR.hasGamePiece() + && !(RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.REST) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.PREPARE_REST))); } public static Command getUnloadCoralCommand() { @@ -43,23 +64,13 @@ public static Command getUnloadCoralCommand() { ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); } - static Command getIntakeSequenceCommand() { - return getInitiateCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral); - } - - private static Command getInitiateCollectionCommand() { + public static Command getIntakeCoralCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.COLLECT) + TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT) ); } - private static Command getAlignCoralCommand() { - return new SequentialCommandGroup( - TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.ALIGN_CORAL).until(RobotContainer.TRANSPORTER::hasCoral), - TransporterCommands.getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState.HOLD_CORAL) - ); - } private static Command getCollectionConfirmationCommand() { return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index 2287965d..111f5d44 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -17,10 +17,10 @@ public class CoralEjectionCommands { public static Command getCoralEjectionCommand() { return GeneralCommands.getContinuousConditionalCommand( - getEjectCoralFromIntakeCommand(), - getEjectCoralFromEndEffectorCommand(), + getEjectCoralFromIntakeCommand().asProxy(), + getEjectCoralFromEndEffectorCommand().asProxy(), () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() - ).onlyIf(SimulationFieldHandler::isHoldingCoral); + ); } private static Command getEjectCoralFromIntakeCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index a7367cf0..b9d2ad76 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -10,6 +10,7 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; @@ -22,19 +23,16 @@ public class CoralPlacingCommands { static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { - return new SequentialCommandGroup( - CoralCollectionCommands.getLoadCoralCommand(), - new ConditionalCommand( - getAutonomouslyScoreCommand(shouldScoreRight), - getScoreCommand(shouldScoreRight), - () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 - ) + return new ConditionalCommand( + getAutonomouslyScoreCommand(shouldScoreRight), + getScoreCommand(), + () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ).onlyIf(CoralCollectionCommands::hasCoral); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)), + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight) || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()), new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) @@ -42,9 +40,10 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { ); } - private static Command getScoreCommand(boolean shouldScoreRight) { + private static Command getScoreCommand() { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), + CoralCollectionCommands.getLoadCoralCommand(), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) @@ -54,7 +53,9 @@ private static Command getScoreCommand(boolean shouldScoreRight) { private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - getPrepareArmElevatorIfWontHitReef(shouldScoreRight), + new SequentialCommandGroup( + CoralCollectionCommands.getLoadCoralCommand(), + getPrepareArmElevatorIfWontHitReef(shouldScoreRight)), new SequentialCommandGroup( getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().until(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), new WaitUntilCommand(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), @@ -78,8 +79,9 @@ private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRigh } private static Command getPrepareArmElevatorIfWontHitReef(boolean shouldScoreRight) { - return GeneralCommands.runWhen( + return GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore), + ArmElevatorCommands.getStayInPlaceCommand(), () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > FieldConstants.SAFE_DISTANCE_FROM_SCORING_POSE_METERS ); } @@ -152,11 +154,11 @@ public static boolean isPrepareArmAngleAboveCurrentArmAngle() { final Rotation2d targetAngle = targetState.prepareState == null ? targetState.targetAngle : targetState.prepareState.targetAngle; - return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle) || RobotContainer.ARM_ELEVATOR.armAtAngle(targetAngle); + return RobotContainer.ARM_ELEVATOR.armAboveAngle(targetAngle); } private static boolean isReadyToScore(boolean shouldScoreRight) { - return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore()) + return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore(), 4) && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 2eb31504..d787ca3c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -32,11 +32,45 @@ public static Command getToggleBrakeCommand() { }).ignoringDisable(true); } + public static Command getToggleShouldLoadCoralCommand() { + return new InstantCommand(() -> { + CoralCollectionCommands.SHOULD_LOAD_CORAL = !CoralCollectionCommands.SHOULD_LOAD_CORAL; + } + ).ignoringDisable(true); + } + + public static Command getToggleShouldManipulateCoralAutonomouslyCommand() { + return new InstantCommand(() -> { + if (CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST || CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) { + CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = false; + CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = false; + } else { + CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = true; + CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true; + } + }).ignoringDisable(true); + } + + public static Command getToggleShouldCollectCoralAtonomouslyCommand() { + return new InstantCommand(() -> { + CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST = !CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST; + } + ).ignoringDisable(true); + } + + public static Command getToggleShouldScoreCoralAtonomouslyCommand() { + return new InstantCommand(() -> { + CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = !CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY; + } + ).ignoringDisable(true); + } + public static Command getDelayedCommand(double delaySeconds, Runnable toRun) { return new WaitCommand(delaySeconds).andThen(toRun).ignoringDisable(true); } - public static Command getContinuousConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { + public static Command getContinuousConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier + condition) { return new ConditionalCommand( onTrue.onlyWhile(condition), onFalse.until(condition), @@ -76,19 +110,23 @@ public static Command getResetFlipArmOverrideCommand() { return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); } - public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, + boolean isPrepareState, BooleanSupplier shouldStartFlipped) { return isPrepareState ? ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } - public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand + (Supplier targetState, boolean isPrepareState, BooleanSupplier + shouldStartFlipped) { return isPrepareState ? ArmElevatorCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : ArmElevatorCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } - public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState) { + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, + boolean isPrepareState) { return isPrepareState ? ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) : ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE); diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index a13abb42..c3aae1c4 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -27,13 +27,13 @@ public class AutonomousConstants { public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate - public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final double FEEDFORWARD_SCALAR = 0.6;//TODO: Calibrate + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final double REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, REEF_RELATIVE_Y_TOLERANCE_METERS = 0.03; - + private static final double AUTO_FIND_CORAL_POSE_X = 3.3, AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5, diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 418efb00..ab6f3596 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -17,20 +17,20 @@ public class CameraConstants { ); private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( - new Translation3d(0.2015, 0.195, 0.7156), - new Rotation3d(0, Units.degreesToRadians(30), 0) + new Translation3d(0.204, -0.170, 0.972), + new Rotation3d(0, Units.degreesToRadians(42), Units.degreesToRadians(15)) ), - ROBOT_CENTER_TO_INTAKE_REEF_TAG_CAMERA = new Transform3d( - new Translation3d(0.2247, 0.195, 0.7498), - new Rotation3d(0, Units.degreesToRadians(90 - 51), 0) + ROBOT_CENTER_TO_FRONT_REEF_TAG_CAMERA = new Transform3d( + new Translation3d(0.221, -0.165, 1.017), + new Rotation3d(0, Units.degreesToRadians(42), Units.degreesToRadians(15)) ), ROBOT_CENTER_TO_LEFT_REEF_TAG_CAMERA = new Transform3d( - new Translation3d(-0.129, 0.2032, 0.1258), - new Rotation3d(0, Units.degreesToRadians(60), 0) + new Translation3d(-0.2032, 0.129, 0.1258), + new Rotation3d(0, Units.degreesToRadians(60), Units.degreesToRadians(180)) ), ROBOT_CENTER_TO_RIGHT_REEF_TAG_CAMERA = new Transform3d( - new Translation3d(-0.129, -0.2032, 0.1258), - new Rotation3d(0, Units.degreesToRadians(60), 0) + new Translation3d(-0.2032, -0.129, 0.1258), + new Rotation3d(0, Units.degreesToRadians(60), Units.degreesToRadians(180)) ); public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 1; @@ -39,10 +39,10 @@ public class CameraConstants { ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); public static final AprilTagCamera - INTAKE_SIDE_REEF_TAG_CAMERA = new AprilTagCamera( + FRONT_REEF_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "IntakeSideReefTagCameraCamera", - ROBOT_CENTER_TO_INTAKE_REEF_TAG_CAMERA, + "FrontReefTagCamera", + ROBOT_CENTER_TO_FRONT_REEF_TAG_CAMERA, REEF_TAG_CAMERA_STANDARD_DEVIATIONS ), LEFT_REEF_TAG_CAMERA = new AprilTagCamera( diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 8c71b297..55b70295 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -28,11 +28,11 @@ public class FieldConstants { public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); public static final double - REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, + REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.35, REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, - REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3; - public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.25; + REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.2; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.17; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index a6ef4925..2fd36cc2 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; +import frc.trigon.robot.commands.commandfactories.ClimbCommands; import frc.trigon.robot.misc.ReefChooser; import lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -18,7 +19,7 @@ public class OperatorConstants { DRIVER_CONTROLLER_PORT = 0, REEF_CHOOSER_PORT = 1; private static final int - DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 1, + DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 2, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT = 2; public static final XboxController DRIVER_CONTROLLER = new XboxController( DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND @@ -36,7 +37,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 1; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ALIGN; public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), @@ -51,19 +52,26 @@ public class OperatorConstants { public static final Trigger FLOOR_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftBumper(),//TODO: Add operator control REEF_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.a()), - STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.povLeft(), + STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.povRight(), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(true, true)), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(false, true)), CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), SCORE_CORAL_RIGHT_TRIGGER = createScoreTrigger(true, false), SCORE_CORAL_LEFT_TRIGGER = createScoreTrigger(false, false), - EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(); + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(), + SHOULD_LOAD_CORAL_TOGGLE_TRIGGER = DRIVER_CONTROLLER.back(), + SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER = DRIVER_CONTROLLER.povLeft(), + SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER = OPERATOR_CONTROLLER.y(), + SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER = OPERATOR_CONTROLLER.t(); public static final Trigger SPAWN_CORAL_IN_SIMULATION_TRIGGER = OPERATOR_CONTROLLER.equals(), FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(), - CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); - + CLIMB_TRIGGER = DRIVER_CONTROLLER.povDown().or(OPERATOR_CONTROLLER.c()); + public static final Trigger + RESET_CLIMBER_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.b()), + RESET_INTAKE_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.n()), + RESET_ELEVATOR_POSITION_TRIGGER = OPERATOR_CONTROLLER.r().and(OPERATOR_CONTROLLER.m()); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a().and(() -> !AlgaeManipulationCommands.isHoldingAlgae())), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()), @@ -79,7 +87,7 @@ public class OperatorConstants { SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeCommand) { - final Trigger scoreTrigger; + Trigger scoreTrigger; if (isRight) scoreTrigger = DRIVER_CONTROLLER.rightStick() @@ -92,6 +100,8 @@ private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeComman .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + scoreTrigger = scoreTrigger.and(() -> !ClimbCommands.isClimbing()); + if (isAlgaeCommand) return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); return scoreTrigger.and(() -> !AlgaeManipulationCommands.isHoldingAlgae()); diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 64ea2cfe..0a27dd57 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -22,9 +22,10 @@ public class ObjectPoseEstimator extends SubsystemBase { /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. * - * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed - * @param gamePieceType the type of game piece to track - * @param camera the camera used for detecting objects + * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed + * @param distanceCalculationMethod the method used to calculate the distance from the game piece + * @param gamePieceType the type of game piece to track + * @param camera the camera used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index ae69acd7..4ac371c5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; -import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; import lib.hardware.RobotHardwareStats; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; import java.util.ArrayList; import java.util.List; @@ -27,7 +27,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8); - private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation()); + private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", true);//TODO: AHAHAHAHAHHA static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 8d0a3fe2..ae8a9d47 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -19,6 +19,7 @@ import org.littletonrobotics.junction.Logger; public class ArmElevator extends MotorSubsystem { + private static final boolean SHOULD_CALIBRATE_ELEVATOR = false; private final TalonFXMotor armMasterMotor = ArmElevatorConstants.ARM_MASTER_MOTOR, elevatorMasterMotor = ArmElevatorConstants.ELEVATOR_MASTER_MOTOR; @@ -30,7 +31,7 @@ public class ArmElevator extends MotorSubsystem { ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION, ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_JERK ).withEnableFOC(ArmElevatorConstants.FOC_ENABLED); - private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( + private final DynamicMotionMagicVoltage elevatorPositionRequest = new DynamicMotionMagicVoltage( 0, ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY, ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION, @@ -45,14 +46,15 @@ public ArmElevator() { @Override public SysIdRoutine.Config getSysIDConfig() { - return ArmElevatorConstants.ARM_SYSID_CONFIG; - //return ArmElevatorConstants.ELEVATOR_SYSID_CONFIG; + return SHOULD_CALIBRATE_ELEVATOR ? ArmElevatorConstants.ELEVATOR_SYSID_CONFIG : ArmElevatorConstants.ARM_SYSID_CONFIG; } @Override public void setBrake(boolean brake) { armMasterMotor.setBrake(brake); + ArmElevatorConstants.ARM_FOLLOWER_MOTOR.setBrake(brake); elevatorMasterMotor.setBrake(brake); + ArmElevatorConstants.ELEVATOR_FOLLOWER_MOTOR.setBrake(brake); } @Override @@ -63,21 +65,24 @@ public void stop() { @Override public void updateLog(SysIdRoutineLog log) { + if (SHOULD_CALIBRATE_ELEVATOR) { + log.motor("Elevator") + .linearPosition(Units.Meters.of(getElevatorPositionRotations())) + .linearVelocity(Units.MetersPerSecond.of(elevatorMasterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ArmElevatorConstants.ELEVATOR_GEAR_RATIO)) + .voltage(Units.Volts.of(elevatorMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + return; + } log.motor("Arm") - .angularPosition(Units.Rotations.of(getCurrentArmAngle().getRotations())) - .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.VELOCITY))) + .angularPosition(Units.Rotations.of(armMasterMotor.getSignal(TalonFXSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(armMasterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ArmElevatorConstants.ARM_GEAR_RATIO)) .voltage(Units.Volts.of(armMasterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); -// log.motor("Elevator") -// .linearPosition(Units.Meters.of(getPositionRotations())) -// .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) -// .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @Override public void updateMechanism() { ArmElevatorConstants.ARM_MECHANISM.update( - Rotation2d.fromRotations(getCurrentArmAngle().getRotations() + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET), - Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET) + Rotation2d.fromRotations(getCurrentArmAngle().getRotations()), + Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET) ); ArmElevatorConstants.ELEVATOR_MECHANISM.update( getCurrentElevatorPositionMeters(), @@ -94,15 +99,24 @@ public void updatePeriodically() { armMasterMotor.update(); angleEncoder.update(); elevatorMasterMotor.update(); - Logger.recordOutput("Elevator/CurrentPositionMeters", getCurrentElevatorPositionMeters()); - Logger.recordOutput("Arm/CurrentPositionDegrees", getCurrentArmAngle().getDegrees()); + Logger.recordOutput("Elevator/ElevatorPositionMeters", getCurrentElevatorPositionMeters()); + Logger.recordOutput("Arm/ArmPositionDegrees", getCurrentArmAngle().getDegrees()); Logger.recordOutput("Arm/isPrepareArmAngleAboveCurrentArmAngle", CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle()); + Logger.recordOutput("Arm/AutonomousScoring", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + Logger.recordOutput("Arm/ShouldLoadCoral", CoralCollectionCommands.SHOULD_LOAD_CORAL); } @Override public void sysIDDrive(double targetVoltage) { + if (SHOULD_CALIBRATE_ELEVATOR) { + elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + return; + } armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); -// elevatorMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + } + + public void resetElevatorPosition() { + elevatorMasterMotor.setPosition(0); } public Pose3d calculateGamePieceCollectionPose() { @@ -125,7 +139,14 @@ public boolean atState(ArmElevatorConstants.ArmElevatorState targetState) { public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { if (targetState == null) return false; - return armAtAngle(isStateReversed ? subtractFrom360Degrees(targetState.targetAngle) : targetState.targetAngle) + return armAtAngle(isStateReversed ? subtractFrom180Degrees(targetState.targetAngle) : targetState.targetAngle) + && elevatorAtPosition(targetState.targetPositionMeters); + } + + public boolean atState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed, double tol) { + if (targetState == null) + return false; + return armAtAngle(isStateReversed ? subtractFrom180Degrees(targetState.targetAngle) : targetState.targetAngle, tol) && elevatorAtPosition(targetState.targetPositionMeters); } @@ -134,8 +155,13 @@ public boolean armAtAngle(Rotation2d targetAngle) { return currentToTargetAngleDifferenceDegrees < ArmElevatorConstants.ANGLE_TOLERANCE.getDegrees(); } + public boolean armAtAngle(Rotation2d targetAngle, double tol) { + final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getCurrentArmAngle()).getDegrees()); + return currentToTargetAngleDifferenceDegrees < tol; + } + public boolean armAboveAngle(Rotation2d targetAngle) { - return targetAngle.getDegrees() < getCurrentArmAngle().getDegrees(); + return targetAngle.getDegrees() - getCurrentArmAngle().getDegrees() < 4; } public boolean elevatorAtPosition(double positionMeters) { @@ -144,7 +170,7 @@ public boolean elevatorAtPosition(double positionMeters) { } public Rotation2d getCurrentArmAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET); } public double getCurrentElevatorPositionMeters() { @@ -186,8 +212,9 @@ void setTargetState(ArmElevatorConstants.ArmElevatorState targetState, boolean i } void setTargetArmState(ArmElevatorConstants.ArmElevatorState targetState, boolean isStateReversed) { - if (isStateReversed) { - setTargetArmAngle(subtractFrom360Degrees(targetState.targetAngle), targetState.ignoreConstraints); + scaleArmPositionRequestSpeed(targetState.speedScalar); + if (isStateReversed && !(targetState == ArmElevatorConstants.ArmElevatorState.SCORE_L1 || targetState == ArmElevatorConstants.ArmElevatorState.PREPARE_SCORE_L1)) { + setTargetArmAngle(subtractFrom180Degrees(targetState.targetAngle), targetState.ignoreConstraints); return; } setTargetArmAngle(targetState.targetAngle, targetState.ignoreConstraints); @@ -199,7 +226,22 @@ void setTargetElevatorState(ArmElevatorConstants.ArmElevatorState targetState) { } void setTargetArmAngle(Rotation2d targetAngle, boolean ignoreConstraints) { - armMasterMotor.setControl(armPositionRequest.withPosition(ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()))); + final Rotation2d minimumSafeAngle = calculateMinimumArmSafeAngle(); + final Rotation2d currentAngle = getCurrentArmAngle(); + if (getElevatorPositionRotations() < metersToRotations(ArmElevatorConstants.ArmElevatorState.REST.targetPositionMeters) && currentAngle.getDegrees() < -80) { + sendControlToArm(-0.25); + return; + } + if (!targetState.ignoreConstraints && minimumSafeAngle.getRotations() > Math.max(currentAngle.getRotations(), targetAngle.getRotations())) { + sendControlToArm(currentAngle.getRotations()); + return; + } + final double targetPosition = ignoreConstraints ? targetAngle.getRotations() : Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()); + sendControlToArm(targetPosition); + } + + void sendControlToArm(double targetRotation) { + armMasterMotor.setControl(armPositionRequest.withPosition(targetRotation - ArmElevatorConstants.ARM_POSITION_OFFSET_FROM_GRAVITY_OFFSET)); } void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignoreConstraints) { @@ -207,7 +249,7 @@ void setTargetElevatorPositionMeters(double targetPositionMeters, boolean ignore } void setTargetElevatorPositionRotations(double targetPositionRotations, boolean ignoreConstraints) { - elevatorMasterMotor.setControl(positionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); + elevatorMasterMotor.setControl(elevatorPositionRequest.withPosition(ignoreConstraints ? targetPositionRotations : Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); } private Rotation2d calculateMinimumArmSafeAngle() { @@ -215,12 +257,12 @@ private Rotation2d calculateMinimumArmSafeAngle() { final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmElevatorConstants.ARM_LENGTH_METERS, 0, 1); final double acos = Math.acos(cosOfMinimumSafeAngle); return Double.isNaN(acos) - ? Rotation2d.fromRadians(0) - : Rotation2d.fromRadians(acos); + ? Rotation2d.fromDegrees(-100) + : Rotation2d.fromRadians(acos).minus(Rotation2d.kCCW_90deg); } private double calculateMinimumSafeElevatorHeightRotations() { - final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getCos(); + final double armCos = RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().plus(Rotation2d.kCCW_90deg).getCos(); final double elevatorHeightFromArm = armCos * ArmElevatorConstants.ARM_LENGTH_METERS; final double minimumElevatorSafeZone = ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; final double minimumSafeHeightMeters = (RobotContainer.ARM_ELEVATOR.isArmAboveSafeAngle() @@ -233,22 +275,32 @@ private double getElevatorHeightFromMinimumSafeZone() { return getCurrentElevatorPositionMeters() - ArmElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } - private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); + private static Rotation2d subtractFrom180Degrees(Rotation2d angleToSubtract) { + return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() - angleToSubtract.getDegrees()); } private Pose3d calculateVisualizationPose() { final Transform3d armTransform = new Transform3d( new Translation3d(0, 0, getCurrentElevatorPositionMeters()), - new Rotation3d(0, getCurrentArmAngle().getRadians(), 0) + new Rotation3d(0, getCurrentArmAngle().getRadians() + Math.PI / 2, 0) ); return ArmElevatorConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); } + private void scaleArmPositionRequestSpeed(double speedScalar) { + armPositionRequest.Velocity = ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_VELOCITY * speedScalar; + armPositionRequest.Acceleration = ArmElevatorConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + armPositionRequest.Jerk = armPositionRequest.Acceleration * 10; + } + private void scaleElevatorPositionRequestSpeed(double speedScalar) { - positionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar; - positionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; - positionRequest.Jerk = positionRequest.Acceleration * 10; + elevatorPositionRequest.Velocity = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_VELOCITY * speedScalar; + elevatorPositionRequest.Acceleration = ArmElevatorConstants.ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + elevatorPositionRequest.Jerk = elevatorPositionRequest.Acceleration * 10; + } + + void setElevatorVoltage(double voltage) { + elevatorMasterMotor.setControl(voltageRequest.withOutput(voltage)); } private Pose3d getFirstStageComponentPose() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 1363f515..e047d5a0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -2,9 +2,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.commands.ExecuteEndCommand; import lib.commands.GearRatioCalculationCommand; import lib.commands.NetworkTablesCommand; @@ -35,6 +40,27 @@ public static Command getArmGearRatioCalulationCommand() { ); } + public static Command resetElevatorPositionCommand() { + return new ParallelCommandGroup( + new ExecuteEndCommand( + () -> RobotContainer.ARM_ELEVATOR.setTargetArmState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR, false), + () -> { + } + ).until(() -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.ZERO_ELEVATOR)), + new ExecuteEndCommand( + () -> RobotContainer.ARM_ELEVATOR.setElevatorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), + () -> { + }, + RobotContainer.ARM_ELEVATOR + ), + SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT) + ).finallyDo(() -> { + RobotContainer.ARM_ELEVATOR.resetElevatorPosition(); + RobotContainer.END_EFFECTOR.setTargetState(EndEffectorConstants.EndEffectorState.REST); + }); + } + public static Command getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState targetState) { return getSetTargetStateCommand(() -> targetState); } @@ -56,6 +82,17 @@ public static Command getSetTargetStateCommand(Supplier { + RobotContainer.ARM_ELEVATOR.setTargetArmAngle(RobotContainer.ARM_ELEVATOR.getCurrentArmAngle(), true); + RobotContainer.ARM_ELEVATOR.setTargetElevatorPositionMeters(RobotContainer.ARM_ELEVATOR.getCurrentElevatorPositionMeters(), true); + }, + RobotContainer.ARM_ELEVATOR::stop, + RobotContainer.ARM_ELEVATOR + ); + } + public static Command getPrepareForStateCommand(Supplier targetState) { return getPrepareForStateCommand(targetState, () -> false); } @@ -69,7 +106,7 @@ public static Command getPrepareForStateCommand(Supplier 0; static final double ELEVATOR_POSITION_TOLERANCE_METERS = 0.02; + static final Trigger IS_BREAKING_ARM_EVENT = new Trigger( + () -> Math.abs(ARM_MASTER_MOTOR.getSignal(TalonFXSignal.STATOR_CURRENT)) > 70 + ).debounce(0.1); + static { configureArmMasterMotor(); configureArmFollowerMotor(); configureElevatorMasterMotor(); configureElevatorFollowerMotor(); configureAngleEncoder(); - configureReverseLimitSensor(); + IS_BREAKING_ARM_EVENT.onTrue(new InstantCommand(ARM_MASTER_MOTOR::stopMotor)); } private static void configureArmMasterMotor() { @@ -194,25 +184,30 @@ private static void configureArmMasterMotor() { config.Feedback.RotorToSensorRatio = ARM_GEAR_RATIO; config.Feedback.FeedbackRemoteSensorID = ANGLE_ENCODER.getID(); - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 34 : 55; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 3 : 1.2; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0.06; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 4.3; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0.358; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; + +// config.Feedback.VelocityFilterTimeConstant = 0.2; config.MotionMagic.MotionMagicCruiseVelocity = ARM_DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(270); +// config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(270); + +// config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Conversions.degreesToRotations(-270); config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT; @@ -225,6 +220,8 @@ private static void configureArmMasterMotor() { ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); + ARM_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); } private static void configureArmFollowerMotor() { @@ -255,22 +252,22 @@ private static void configureElevatorMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = ELEVATOR_GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 22; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 1; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0.0546875; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0.4; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0;//0.036238 + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0.41015625; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; +// config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ELEVATOR_REVERSE_LIMIT_RESET_POSITION_ROTATIONS; +// +// config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; config.MotionMagic.MotionMagicCruiseVelocity = ELEVATOR_DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = ELEVATOR_DEFAULT_MAXIMUM_ACCELERATION; @@ -287,6 +284,8 @@ private static void configureElevatorMasterMotor() { ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + ELEVATOR_MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); +// ELEVATOR_MASTER_MOTOR.setPosition(0); } private static void configureElevatorFollowerMotor() { @@ -310,9 +309,9 @@ private static void configureElevatorFollowerMotor() { private static void configureAngleEncoder() { final CANcoderConfiguration config = new CANcoderConfiguration(); - config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_GRAVITY_OFFSET; - config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; ANGLE_ENCODER.applyConfiguration(config); ANGLE_ENCODER.setSimulationInputsFromTalonFX(ARM_MASTER_MOTOR); @@ -321,33 +320,34 @@ private static void configureAngleEncoder() { ANGLE_ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); } - private static void configureReverseLimitSensor() { - REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> ELEVATOR_MASTER_MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); - } - public enum ArmElevatorState { - PREPARE_SCORE_L1(Rotation2d.fromDegrees(110), 0.29, null, false, 1), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(100), 0.29, null, false, 1), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(100), 0.69, null, false, 1), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(120), 1.19, null, false, 1), - REST(Rotation2d.fromDegrees(0), 0.593, null, false, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(180), 0.593, null, false, 0.7), - REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.593, null, false, 0.7), - REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.593, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5419, REST, true, 0.7), - UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.5419, null, false, 0.7), - EJECT(Rotation2d.fromDegrees(60), 0.593, null, false, 0.7), - SCORE_L1(Rotation2d.fromDegrees(70), 0.39, null, false, 1), - SCORE_L2(Rotation2d.fromDegrees(90), 0.29, PREPARE_SCORE_L2, false, 1), - SCORE_L3(Rotation2d.fromDegrees(90), 0.69, PREPARE_SCORE_L3, false, 1), - SCORE_L4(Rotation2d.fromDegrees(100), 1.19, PREPARE_SCORE_L4, false, 1), - SCORE_NET(Rotation2d.fromDegrees(160), 1.372, null, false, 0.3), - SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.09, null, false, 0.7), - COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.593, null, false, 1), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.943, null, false, 1), - COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.29, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(70), 0.09, null, true, 1); + PREPARE_SCORE_L1(Rotation2d.fromDegrees(-8), 0.5, null, false, 1), + PREPARE_SCORE_L2(Rotation2d.fromDegrees(60), 0.13, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(60), 0.6, null, false, 1), + PREPARE_SCORE_L4(Rotation2d.fromDegrees(50), 1.5, null, false, 1), + PREPARE_REST(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.8), + REST(Rotation2d.fromDegrees(-90), 0.534, PREPARE_REST, true, 0.8), + REST_AFTER_LOADING(Rotation2d.fromDegrees(-90), 0.603, null, true, 0.7), + REST_WITH_CORAL(Rotation2d.fromDegrees(90), 0.603, null, false, 0.8), + REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.3), + PREPARE_FOR_REST_FOR_CLIMB(Rotation2d.fromDegrees(-29), 0.6, null, false, 0.7), + REST_FOR_CLIMB(Rotation2d.fromDegrees(-29), 0, PREPARE_FOR_REST_FOR_CLIMB, true, 0.7), + ZERO_ELEVATOR(Rotation2d.fromDegrees(90), 0.65, null, false, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(-90), 0.485, null, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(-90), 0.603, null, false, 0.7), + EJECT(Rotation2d.fromDegrees(-30), 0.603, null, false, 0.7), + SCORE_L1(Rotation2d.fromDegrees(-13), PREPARE_SCORE_L1.targetPositionMeters, null, false, 1), + SCORE_L2(Rotation2d.fromDegrees(25), PREPARE_SCORE_L2.targetPositionMeters, PREPARE_SCORE_L2, false, 0.8), + SCORE_L3(Rotation2d.fromDegrees(25), PREPARE_SCORE_L3.targetPositionMeters, PREPARE_SCORE_L3, false, 0.8), + SCORE_L4(Rotation2d.fromDegrees(-5), 1.5, PREPARE_SCORE_L4, false, 0.8), + SCORE_NET(Rotation2d.fromDegrees(70), 1.61, null, false, 0.3), + SCORE_PROCESSOR(Rotation2d.fromDegrees(0), 0.1, null, false, 0.4), + COLLECT_ALGAE_L2(Rotation2d.fromDegrees(0), 0.603, null, false, 1), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(0), 1.15, null, false, 1), + PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.35, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(-32), 0.22, PREPARE_COLLECT_ALGAE_FLOOR, true, 1), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(0), 0.15, null, false, 1); + public final Rotation2d targetAngle; public final double targetPositionMeters; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index c112dbd0..ad111523 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -10,6 +10,8 @@ import lib.hardware.misc.servo.Servo; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Climber extends MotorSubsystem { @@ -28,7 +30,7 @@ public Climber() { @Override public void stop() { motor.stopMotor(); - setServoPowers(0); + stopServos(); } @Override @@ -45,7 +47,7 @@ public void updateMechanism() { public void updatePeriodically() { motor.update(); rightServo.update(); - leftServo.update(); + // leftServo.update(); } @Override @@ -79,10 +81,6 @@ public boolean atTargetState() { return Math.abs(getPositionRotations() - targetState.targetPositionRotations) < ClimberConstants.CLIMBER_TOLERANCE_ROTATIONS; } - public boolean hasCage() { - return ClimberConstants.HAS_CAGE_BOOLEAN_EVENT.getAsBoolean(); - } - void setTargetState(ClimberConstants.ClimberState targetState) { this.targetState = targetState; setTargetState(targetState.targetPositionRotations, targetState.targetServoPower, targetState.isAffectedByRobotWeight); @@ -100,11 +98,20 @@ void setTargetVoltage(double targetVoltage) { motor.setControl(voltageRequest.withOutput(targetVoltage)); } - private void setServoPowers(double power) { + void resetClimberPosition() { + motor.setPosition(0); + } + + public void setServoPowers(double power) { rightServo.setTargetSpeed(power); leftServo.setTargetSpeed(-power); } + private void stopServos() { + rightServo.stop(); + leftServo.stop(); + } + private Pose3d calculateVisualizationPose() { final Transform3d climberTransform = new Transform3d( new Translation3d(0, 0, 0), @@ -113,7 +120,7 @@ private Pose3d calculateVisualizationPose() { return ClimberConstants.CLIMBER_VISUALIZATION_ORIGIN_POINT.transformBy(climberTransform); } - + @AutoLogOutput(key = "Climber/ClimberPosition") private double getPositionRotations() { return motor.getSignal(TalonFXSignal.POSITION); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 34b3970a..bc68eabd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.commands.ExecuteEndCommand; import lib.commands.NetworkTablesCommand; @@ -20,6 +22,15 @@ public static Command getDebuggingCommand() { ); } + public static Command resetClimberPositionCommand() { + return new ExecuteEndCommand( + () -> RobotContainer.CLIMBER.setTargetVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 3), + RobotContainer.CLIMBER::resetClimberPosition, + RobotContainer.CLIMBER + ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); + } + + public static Command getSetTargetStateCommand(ClimberConstants.ClimberState targetState) { return new StartEndCommand( () -> RobotContainer.CLIMBER.setTargetState(targetState), @@ -43,4 +54,13 @@ public static Command getSetTargetSpeedCommand(DoubleSupplier targetSpeed) { RobotContainer.CLIMBER ); } + + public static Command getDefaultCommand() { + return new StartEndCommand( + RobotContainer.CLIMBER::stop, + () -> { + }, + RobotContainer.CLIMBER + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 9d2e6996..7a6427be 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.climber; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -9,53 +10,39 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.servo.Servo; -import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.hardware.simulation.SimpleMotorSimulation; import lib.utilities.mechanisms.SingleJointedArmMechanism2d; -import java.util.function.DoubleSupplier; - public class ClimberConstants { private static final int MOTOR_ID = 18, - REVERSE_LIMIT_SENSOR_CHANNEL = 0, - RIGHT_SERVO_CHANNEL = 1, - LEFT_SERVO_CHANNEL = 2, - CAGE_SENSOR_CHANNEL = 3; + RIGHT_SERVO_CHANNEL = 0, + LEFT_SERVO_CHANNEL = 1; private static final String MOTOR_NAME = "ClimberMotor", - REVERSE_LIMIT_SENSOR_NAME = "ClimberReverseLimitSensor", RIGHT_SERVO_NAME = "ClimberRightServo", - LEFT_SERVO_NAME = "ClimberLeftServo", - CAGE_SENSOR_NAME = "ClimberCageSensor"; + LEFT_SERVO_NAME = "ClimberLeftServo"; static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); static final Servo RIGHT_SERVO = new Servo(RIGHT_SERVO_CHANNEL, RIGHT_SERVO_NAME), LEFT_SERVO = new Servo(LEFT_SERVO_CHANNEL, LEFT_SERVO_NAME); - private static final SimpleSensor - REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SENSOR_NAME), - CAGE_SENSOR = SimpleSensor.createDigitalSensor(CAGE_SENSOR_CHANNEL, CAGE_SENSOR_NAME); static final int GROUNDED_PID_SLOT = 0, ON_CAGE_PID_SLOT = 1; private static final double GEAR_RATIO = 37.5; - private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0; - private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3; - private static final int - SERVO_PULSE_WIDTH_MICROSECONDS = 20000, - SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS = 0, - SERVO_CENTER_PULSE_WIDTH_MICROSECONDS = 1500, - SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS = 1000, - SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS = 2000; +// private static final int +// SERVO_PULSE_WIDTH_MICROSECONDS = 20000, +// SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS = 0, +// SERVO_CENTER_PULSE_WIDTH_MICROSECONDS = 1500, +// SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS = 1000, +// SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS = 2000; static final boolean FOC_ENABLED = true; @@ -86,25 +73,11 @@ public class ClimberConstants { ); static final double MAXIMUM_MANUAL_CONTROL_VOLTAGE = 4; - private static final double - HAS_CAGE_DEBOUNCE_TIME_SECONDS = 0.5, - REVERSE_LIMIT_DEBOUNCE_TIME_SECONDS = 0.1; - static final BooleanEvent - HAS_CAGE_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - CAGE_SENSOR::getBinaryValue - ).debounce(HAS_CAGE_DEBOUNCE_TIME_SECONDS), - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - REVERSE_LIMIT_SENSOR::getBinaryValue - ).debounce(REVERSE_LIMIT_DEBOUNCE_TIME_SECONDS); - private static final DoubleSupplier REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER = () -> 0; - static final double CLIMBER_TOLERANCE_ROTATIONS = 0.01; + static final double CLIMBER_TOLERANCE_ROTATIONS = 0.02; static { configureMotor(); configureServos(); - configureReverseLimitSensor(); } private static void configureMotor() { @@ -113,31 +86,27 @@ private static void configureMotor() { config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 22.373 : 70; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.33014 : 0; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016057 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 2.2; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0 : -0.2001953125; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.074561 : 0; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; - config.Slot1.kP = RobotHardwareStats.isSimulation() ? 22.373 : 0; + config.Slot1.kP = RobotHardwareStats.isSimulation() ? 22.373 : 80; config.Slot1.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot1.kD = RobotHardwareStats.isSimulation() ? 0.33014 : 0; config.Slot1.kS = RobotHardwareStats.isSimulation() ? 0.016057 : 0; - config.Slot1.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 0; + config.Slot1.kV = RobotHardwareStats.isSimulation() ? 4.3932 : 2.2; + config.Slot1.kG = RobotHardwareStats.isSimulation() ? 0 : -1; config.Slot1.kA = RobotHardwareStats.isSimulation() ? 0.074561 : 0; - config.Slot1.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - - config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 2 : 2; - config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 10 : 10; - config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + config.Slot1.GravityType = GravityTypeValue.Elevator_Static; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS; + config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 2 : 1.5; + config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 10 : 3; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -149,36 +118,30 @@ private static void configureMotor() { MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); - MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); } private static void configureServos() { - RIGHT_SERVO.setPWMBoundaries( - SERVO_PULSE_WIDTH_MICROSECONDS, - SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, - SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS - ); - LEFT_SERVO.setPWMBoundaries( - SERVO_PULSE_WIDTH_MICROSECONDS, - SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, - SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, - SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS - ); - } - - private static void configureReverseLimitSensor() { - REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER); - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS)); +// RIGHT_SERVO.setPWMBoundaries( +// SERVO_PULSE_WIDTH_MICROSECONDS, +// SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, +// SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS +// ); +// LEFT_SERVO.setPWMBoundaries( +// SERVO_PULSE_WIDTH_MICROSECONDS, +// SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_CENTER_PULSE_WIDTH_MICROSECONDS, +// SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS, +// SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS +// ); } public enum ClimberState { REST(0, 0, false), - PREPARE_FOR_CLIMB(3, 1, false), - CLIMB(0, 0, true); + BREAK_ZIP_TIE(-0.35, 0, false), + PREPARE_FOR_CLIMB(0, 1, false), + CLIMB(-1.3, 0, true); public final double targetPositionRotations; public final double targetServoPower; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index ff915dd4..ee741d82 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -1,5 +1,6 @@ package frc.trigon.robot.subsystems.endeffector; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Translation3d; import frc.trigon.robot.RobotContainer; @@ -31,9 +32,10 @@ public void updatePeriodically() { EndEffectorConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("EndEffector/isHoldingAlgae", AlgaeManipulationCommands.isHoldingAlgae()); + Logger.recordOutput("EndEffector/EndEffectorSensorCM", EndEffectorConstants.DISTANCE_SENSOR.getScaledValue()); } - @AutoLogOutput(key = "EndEffector/HasCoral") + @AutoLogOutput(key = "EndEffector/EndEffectorHasCoral") public boolean hasGamePiece() { return EndEffectorConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } @@ -56,17 +58,25 @@ public Translation3d calculateLinearEndEffectorVelocity() { @AutoLogOutput(key = "EndEffector/IsEjecting") public boolean isEjecting() { - return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; + return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 1; } - void setTargetState(EndEffectorConstants.EndEffectorState targetState) { - setEndEffectorTargetVoltage(targetState.targetVoltage); + public void setTargetState(EndEffectorConstants.EndEffectorState targetState) { + if (targetState == EndEffectorConstants.EndEffectorState.HOLD_ALGAE) { + setCur(targetState.targetVoltage); + } else { + setEndEffectorTargetVoltage(targetState.targetVoltage); + } } void setTargetState(double targetVoltage) { setEndEffectorTargetVoltage(targetVoltage); } + private void setCur(double cur) { + endEffectorMotor.setControl(new TorqueCurrentFOC(cur).withMaxAbsDutyCycle(0.6)); + } + private void setEndEffectorTargetVoltage(double targetVoltage) { EndEffectorConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java index 93455350..d6d1c8f5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorCommands.java @@ -1,8 +1,10 @@ package frc.trigon.robot.subsystems.endeffector; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; import lib.commands.NetworkTablesCommand; import java.util.Set; @@ -20,8 +22,16 @@ public static Command getDebuggingCommand() { public static Command getSetTargetStateCommand(EndEffectorConstants.EndEffectorState targetState) { return new StartEndCommand( () -> RobotContainer.END_EFFECTOR.setTargetState(targetState), - RobotContainer.END_EFFECTOR::stop, + () -> {}, RobotContainer.END_EFFECTOR ); } + + public static Command getDefaultCommand() { + return GeneralCommands.getContinuousConditionalCommand( + getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_CORAL), + getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST), + RobotContainer.END_EFFECTOR::hasGamePiece + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index 2b1ff62b..df134cef 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -18,12 +19,12 @@ public class EndEffectorConstants { private static final int END_EFFECTOR_MOTOR_ID = 15, - DISTANCE_SENSOR_CHANNEL = 4; + DISTANCE_SENSOR_CHANNEL = 0; private static final String END_EFFECTOR_MOTOR_NAME = "EndEffectorMotor", DISTANCE_SENSOR_NAME = "EndEffectorDistanceSensor"; - static final TalonFXMotor END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME); - static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); + static final TalonFXMotor END_EFFECTOR_MOTOR = new TalonFXMotor(END_EFFECTOR_MOTOR_ID, END_EFFECTOR_MOTOR_NAME, RobotConstants.CANIVORE_NAME); + static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDutyCycleSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); static final boolean FOC_ENABLED = true; private static final double END_EFFECTOR_GEAR_RATIO = 12.82; @@ -38,17 +39,21 @@ public class EndEffectorConstants { END_EFFECTOR_MOMENT_OF_INERTIA ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 10; static final SpeedMechanism2d END_EFFECTOR_MECHANISM = new SpeedMechanism2d( "EndEffectorMechanism", END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY ); - private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.16; + private static final double + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 4; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - DISTANCE_SENSOR::getBinaryValue + () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); @@ -62,8 +67,8 @@ private static void configureEndEffectorMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -80,6 +85,7 @@ private static void configureEndEffectorMotor() { } private static void configureDistanceSensor() { + DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); } @@ -88,10 +94,11 @@ public enum EndEffectorState { EJECT(4), LOAD_CORAL(-4), UNLOAD_CORAL(4), - SCORE_CORAL(4), - COLLECT_ALGAE(-4), - SCORE_ALGAE(4), - HOLD_ALGAE(-4); + SCORE_CORAL(1.5), + COLLECT_ALGAE(-11), + SCORE_ALGAE(10), + HOLD_CORAL(-0.5), + HOLD_ALGAE(-50); public final double targetVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 90505deb..ad5fca7d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; @@ -51,9 +52,10 @@ public void setBrake(boolean brake) { public void updatePeriodically() { intakeMotor.update(); angleMotor.update(); - IntakeConstants.FORWARD_LIMIT_SENSOR.updateSensor(); - IntakeConstants.REVERSE_LIMIT_SENSOR.updateSensor(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); + Logger.recordOutput("Intake/IntakeSensorCM", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); + Logger.recordOutput("Intake/IntakeAngle", getCurrentAngle().getDegrees()); + Logger.recordOutput("Intake/AutonomousIntake", CoralCollectionCommands.SHOULD_USE_INTAKE_ASSIST); } @Override @@ -70,7 +72,7 @@ public void updateMechanism() { @Override public void stop() { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(0); - intakeMotor.stopMotor(); +// intakeMotor.stopMotor(); angleMotor.stopMotor(); } @@ -78,13 +80,13 @@ public boolean atState(IntakeConstants.IntakeState targetState) { return targetState == this.targetState && atTargetAngle(); } - @AutoLogOutput(key = "CoralIntake/AtTargetAngle") + @AutoLogOutput(key = "Intake/AtTargetAngle") public boolean atTargetAngle() { final double angleDifferenceFromTargetAngleDegrees = Math.abs(getCurrentAngle().minus(targetState.targetAngle).getDegrees()); return angleDifferenceFromTargetAngleDegrees < IntakeConstants.ANGLE_TOLERANCE.getDegrees(); } - @AutoLogOutput(key = "CoralIntake/HasCoral") + @AutoLogOutput(key = "Intake/IntakeHasCoral") public boolean hasCoral() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } @@ -97,7 +99,7 @@ void prepareForState(IntakeConstants.IntakeState targetState) { targetState.targetVoltage ); } - + public Translation3d calculateLinearIntakeVelocity() { double velocityMetersPerSecond = intakeMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * IntakeConstants.WHEEL_RADIUS_METERS; return new Translation3d( @@ -107,7 +109,11 @@ public Translation3d calculateLinearIntakeVelocity() { ); } - void setTargetState(IntakeConstants.IntakeState targetState) { + public void resetIntakePosition() { + angleMotor.setPosition(0); + } + + public void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; setTargetState(targetState.targetAngle, targetState.targetVoltage); } @@ -117,6 +123,10 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetAngle(targetAngle); } + void setAngleMotorVoltage(double voltage) { + angleMotor.setControl(voltageRequest.withOutput(voltage)); + } + private void setTargetVoltage(double voltage) { IntakeConstants.INTAKE_MECHANISM.setTargetVelocity(voltage); intakeMotor.setControl(voltageRequest.withOutput(voltage)); @@ -129,7 +139,7 @@ private void setTargetAngle(Rotation2d targetAngle) { private Pose3d calculateVisualizationPose() { final Transform3d transform = new Transform3d( new Translation3d(), - new Rotation3d(0, getCurrentAngle().getRadians(), 0) + new Rotation3d(0, -getCurrentAngle().getRadians(), 0) ); return IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.transformBy(transform); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 9194dbc2..fb84bca9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -2,8 +2,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import lib.commands.ExecuteEndCommand; import lib.commands.NetworkTablesCommand; import java.util.Set; @@ -19,6 +24,14 @@ public static Command getDebuggingCommand() { ); } + public static Command resetIntakePositionCommand() { + return new ExecuteEndCommand( + () -> RobotContainer.INTAKE.setAngleMotorVoltage(OperatorConstants.DRIVER_CONTROLLER.getRightY() * 2), + RobotContainer.INTAKE::resetIntakePosition, + RobotContainer.INTAKE + ).alongWith(SwerveCommands.getOpenLoopFieldRelativeDriveCommand(() -> 0, () -> 0, () -> 0)); + } + public static Command getPrepareForStateCommand(IntakeConstants.IntakeState targetState) { return new StartEndCommand( () -> RobotContainer.INTAKE.prepareForState(targetState), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 1581dbec..a1a263c3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -31,22 +31,16 @@ public class IntakeConstants { private static final int INTAKE_MOTOR_ID = 9, ANGLE_MOTOR_ID = 10, - REVERSE_LIMIT_SENSOR_CHANNEL = 0, - FORWARD_LIMIT_CHANNEL = 1, - DISTANCE_SENSOR_CHANNEL = 2; + DISTANCE_SENSOR_CHANNEL = 3; private static final String INTAKE_MOTOR_NAME = "IntakeMotor", ANGLE_MOTOR_NAME = "IntakeAngleMotor", - REVERSE_LIMIT_SWITCH_NAME = "IntakeReverseLimitSwitch", - FORWARD_LIMIT_SWITCH_NAME = "IntakeForwardLimitSwitch", DISTANCE_SENSOR_NAME = "IntakeDistanceSensor"; static final TalonFXMotor INTAKE_MOTOR = new TalonFXMotor(INTAKE_MOTOR_ID, INTAKE_MOTOR_NAME), ANGLE_MOTOR = new TalonFXMotor(ANGLE_MOTOR_ID, ANGLE_MOTOR_NAME); static final SimpleSensor - REVERSE_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(REVERSE_LIMIT_SENSOR_CHANNEL, REVERSE_LIMIT_SWITCH_NAME), - FORWARD_LIMIT_SENSOR = SimpleSensor.createDigitalSensor(FORWARD_LIMIT_CHANNEL, FORWARD_LIMIT_SWITCH_NAME), - DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); + DISTANCE_SENSOR = SimpleSensor.createDutyCycleSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME); private static final double INTAKE_MOTOR_GEAR_RATIO = 4, @@ -64,8 +58,8 @@ public class IntakeConstants { INTAKE_LENGTH_METERS = 0.365, INTAKE_MASS_KILOGRAMS = 3.26; private static final Rotation2d - MINIMUM_ANGLE = Rotation2d.fromDegrees(-12), - MAXIMUM_ANGLE = Rotation2d.fromDegrees(110); + MINIMUM_ANGLE = Rotation2d.fromDegrees(-127), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(0); private static final boolean SHOULD_SIMULATE_GRAVITY = true; private static final SimpleMotorSimulation INTAKE_SIMULATION = new SimpleMotorSimulation( INTAKE_GEARBOX, @@ -81,14 +75,11 @@ public class IntakeConstants { MAXIMUM_ANGLE, SHOULD_SIMULATE_GRAVITY ); - private static final DoubleSupplier - REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0, - FORWARD_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0, - DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 1000; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(0.2).per(Units.Second), - Units.Volts.of(0.6), + Units.Volts.of(1.2).per(Units.Second), + Units.Volts.of(1.5), null ); @@ -109,23 +100,15 @@ public class IntakeConstants { ); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1.5); + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.04; private static final double - COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2, - REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1, - FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; + private static final double COLLECTION_DETECTION_DISTANCE_CENTIMETRES = 35; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - DISTANCE_SENSOR::getBinaryValue + () -> DISTANCE_SENSOR.getScaledValue() < COLLECTION_DETECTION_DISTANCE_CENTIMETRES ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); - private static final BooleanEvent - REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - REVERSE_LIMIT_SENSOR::getBinaryValue - ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS), - FORWARD_LIMIT_SENSOR_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - FORWARD_LIMIT_SENSOR::getBinaryValue - ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); public static Pose3d CORAL_COLLECTION_POSE = new Pose3d( new Translation3d(0.6827, 0, 0), new Rotation3d() @@ -135,8 +118,6 @@ public class IntakeConstants { static { configureIntakeMotor(); configureAngleMotor(); - configureLimitSensor(REVERSE_LIMIT_SENSOR, REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER, REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT, MINIMUM_ANGLE); - configureLimitSensor(FORWARD_LIMIT_SENSOR, FORWARD_LIMIT_SENSOR_SIMULATION_SUPPLIER, FORWARD_LIMIT_SENSOR_BOOLEAN_EVENT, MAXIMUM_ANGLE); configureDistanceSensor(); } @@ -149,7 +130,7 @@ private static void configureIntakeMotor() { config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.RotorToSensorRatio = INTAKE_MOTOR_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = INTAKE_MOTOR_GEAR_RATIO; config.CurrentLimits.SupplyCurrentLimit = 60; INTAKE_MOTOR.applyConfiguration(config); @@ -169,28 +150,28 @@ private static void configureAngleMotor() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.RotorToSensorRatio = ANGLE_MOTOR_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ANGLE_MOTOR_GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 500 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 500 : 30; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0054454 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 3.3247 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0.5; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0054454 : 0.61; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 3.3247 : 4; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.047542 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.35427 : 0.32151; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.35427 : 0.3; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 12 / config.Slot0.kV : 2; + config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 12 / config.Slot0.kV : 3; config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 8 : 8; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MINIMUM_ANGLE.getRotations(); - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAXIMUM_ANGLE.getRotations(); +// config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MINIMUM_ANGLE.getRotations(); +// +// config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; +// config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAXIMUM_ANGLE.getRotations(); config.CurrentLimits.SupplyCurrentLimit = 60; @@ -204,24 +185,20 @@ private static void configureAngleMotor() { ANGLE_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); ANGLE_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); ANGLE_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); - } - - private static void configureLimitSensor(SimpleSensor limitSensor, DoubleSupplier simulationSupplier, BooleanEvent booleanEvent, Rotation2d resetPosition) { - limitSensor.setSimulationSupplier(simulationSupplier); - booleanEvent.ifHigh(() -> ANGLE_MOTOR.setPosition(resetPosition.getRotations())); +// OperatorConstants.DEBUGGING_TRIGGER.onTrue(new InstantCommand(() -> ANGLE_MOTOR.setPosition(0)).ignoringDisable(true)); } private static void configureDistanceSensor() { DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_SUPPLIER); + DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); } public enum IntakeState { - REST(0, MINIMUM_ANGLE), - OPEN_REST(0, MAXIMUM_ANGLE), - REST_FOR_CLIMB(0, MAXIMUM_ANGLE), - COLLECT(5, MAXIMUM_ANGLE), - EJECT(-5, MAXIMUM_ANGLE); + REST(0, MAXIMUM_ANGLE), + OPEN_REST(0, MINIMUM_ANGLE), + COLLECT(-8, MINIMUM_ANGLE), + EJECT(5, MAXIMUM_ANGLE); public final double targetVoltage; public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 8789cf5d..59ec58dc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -1,7 +1,5 @@ package frc.trigon.robot.subsystems.swerve; -import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.swerve.SwerveSetpoint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -304,10 +302,12 @@ private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaP } private ChassisSpeeds calculateSelfRelativePIDToPoseSpeeds(FlippablePose2d targetPose) { - final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Pose2d currentPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotFuturePose(0.13); final Pose2d flippedTargetPose = targetPose.get(); - final double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); - final double ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), flippedTargetPose.getY()); + double xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), flippedTargetPose.getX()); + xSpeed = SwerveConstants.X_TRANSLATION_PID_CONTROLLER.atSetpoint() ? 0 : xSpeed; + double ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), flippedTargetPose.getY()); + ySpeed = SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.atSetpoint() ? 0 : ySpeed; final int direction = Flippable.isRedAlliance() ? -1 : 1; final ChassisSpeeds targetFieldRelativeSpeeds = new ChassisSpeeds( xSpeed * direction, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 4e2e430b..e3833e87 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -28,15 +28,15 @@ public class SwerveConstants { REAR_LEFT_ID = 3, REAR_RIGHT_ID = 4; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = 0, - FRONT_RIGHT_STEER_ENCODER_OFFSET = 0, - REAR_LEFT_STEER_ENCODER_OFFSET = 0, - REAR_RIGHT_STEER_ENCODER_OFFSET = 0; + FRONT_LEFT_STEER_ENCODER_OFFSET = -0.044677734375, + FRONT_RIGHT_STEER_ENCODER_OFFSET = 0.3525390625, + REAR_LEFT_STEER_ENCODER_OFFSET = -0.30517578125, + REAR_RIGHT_STEER_ENCODER_OFFSET = -0.121826171875; private static final double - FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2, - FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2, - REAR_LEFT_WHEEL_DIAMETER = 0.05 * 2, - REAR_RIGHT_WHEEL_DIAMETER = 0.05 * 2; + FRONT_LEFT_WHEEL_DIAMETER = 0.1016, + FRONT_RIGHT_WHEEL_DIAMETER = 0.1016, + REAR_LEFT_WHEEL_DIAMETER = 0.1016, + REAR_RIGHT_WHEEL_DIAMETER = 0.1016; static final SwerveModule[] SWERVE_MODULES = new SwerveModule[]{ new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER), new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER), @@ -61,10 +61,10 @@ public class SwerveConstants { private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : - new PIDConstants(4.2, 0, 0), + new PIDConstants(6.3, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0) : - new PIDConstants(13, 0, 0.25); + new PIDConstants(10, 0, 0.1); private static final double MAXIMUM_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : Units.radiansToDegrees(MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND), MAXIMUM_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 900; @@ -95,13 +95,15 @@ public class SwerveConstants { configureGyro(); SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.enableContinuousInput(-SwerveConstants.MAXIMUM_PID_ANGLE, SwerveConstants.MAXIMUM_PID_ANGLE); SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setTolerance(1); + SwerveConstants.X_TRANSLATION_PID_CONTROLLER.setTolerance(0.02); + SwerveConstants.Y_TRANSLATION_PID_CONTROLLER.setTolerance(0.02); } private static void configureGyro() { final Pigeon2Configuration config = new Pigeon2Configuration(); - config.MountPose.MountPoseYaw = 0; - config.MountPose.MountPosePitch = 0; - config.MountPose.MountPoseRoll = 0; + config.MountPose.MountPoseYaw = Units.degreesToRotations(-52.198792); + config.MountPose.MountPosePitch = Units.degreesToRadians(-0.087891); + config.MountPose.MountPoseRoll = Units.degreesToRadians(-0.659180); GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index b7432bf2..2981bd8a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -31,7 +31,7 @@ public class SwerveModuleConstants { public static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1).per(Units.Second), - Units.Volts.of(8), + Units.Volts.of(2), Units.Second.of(1000) ); @@ -77,12 +77,12 @@ public static TalonFXConfiguration generateDriveMotorConfiguration() { config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 50; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 5.25; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0.2; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0.91963; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0.069151; config.Feedback.VelocityFilterTimeConstant = 0; @@ -98,14 +98,14 @@ static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSe config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 30; + config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 50; config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.RotorToSensorRatio = REAR_STEER_MOTOR_GEAR_RATIO; config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; config.Feedback.FeedbackRemoteSensorID = feedbackRemoteSensorID; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 65; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 40; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; config.ClosedLoopGeneral.ContinuousWrap = true; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java index abbdacce..2d6c2b99 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java @@ -42,12 +42,12 @@ public TransporterConstants.TransporterState getTargetState() { return targetState; } - @AutoLogOutput(key = "Transporter/hasCoral") + @AutoLogOutput(key = "Transporter/TransporterHasCoral") public boolean hasCoral() { return TransporterConstants.HAS_CORAL_BOOLEAN_EVENT.getAsBoolean(); } - void setTargetState(TransporterConstants.TransporterState targetState) { + public void setTargetState(TransporterConstants.TransporterState targetState) { this.targetState = targetState; setTargetState(targetState.targetRightMotorVoltage, targetState.targetLeftMotorVoltage); } diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java index 95e32ae0..f9e2b343 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterCommands.java @@ -1,6 +1,9 @@ package frc.trigon.robot.subsystems.transporter; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.trigon.robot.RobotContainer; import lib.commands.NetworkTablesCommand; @@ -18,18 +21,16 @@ public static Command getDebuggingCommand() { } public static Command getSetTargetStateWithPulsesCommand(TransporterConstants.TransporterState targetState) { - return new RepeatCommand( - new SequentialCommandGroup( - getSetTargetStateCommand(targetState).withTimeout(TransporterConstants.PULSE_VOLTAGE_APPLIED_TIME_SECONDS), - new WaitCommand(TransporterConstants.PULSE_WAIT_TIME_SECONDS) - ) - ); + return new SequentialCommandGroup( + getSetTargetStateCommand(targetState).withTimeout(TransporterConstants.PULSE_VOLTAGE_APPLIED_TIME_SECONDS), + new WaitCommand(TransporterConstants.PULSE_WAIT_TIME_SECONDS) + ).repeatedly(); } public static Command getSetTargetStateCommand(TransporterConstants.TransporterState targetState) { return new StartEndCommand( () -> RobotContainer.TRANSPORTER.setTargetState(targetState), - RobotContainer.TRANSPORTER::stop, + () -> {}, RobotContainer.TRANSPORTER ); } @@ -37,7 +38,7 @@ public static Command getSetTargetStateCommand(TransporterConstants.TransporterS public static Command getSetTargetStateCommand(double rightMotorVoltage, double leftMotorVoltage) { return new StartEndCommand( () -> RobotContainer.TRANSPORTER.setTargetState(rightMotorVoltage, leftMotorVoltage), - RobotContainer.TRANSPORTER::stop, + () -> {}, RobotContainer.TRANSPORTER ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 6dcf7d90..261fbc3e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -22,7 +22,7 @@ public class TransporterConstants { private static final int RIGHT_MOTOR_ID = 11, LEFT_MOTOR_ID = 12, - BEAM_BREAK_CHANNEL = 3; + BEAM_BREAK_CHANNEL = 1; private static final String RIGHT_MOTOR_NAME = "TransporterRightMotor", LEFT_MOTOR_NAME = "TransporterLeftMotor", @@ -32,10 +32,10 @@ public class TransporterConstants { LEFT_MOTOR = new TalonFXMotor(LEFT_MOTOR_ID, LEFT_MOTOR_NAME); static final SimpleSensor BEAM_BREAK = SimpleSensor.createDigitalSensor(BEAM_BREAK_CHANNEL, BEAM_BREAK_NAME); - private static double GEAR_RATIO = 3; - static boolean FOC_ENABLED = true; - private static int MOTOR_AMOUNT = 1; - private static DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); + private final static double GEAR_RATIO = 3; + static final boolean FOC_ENABLED = true; + private final static int MOTOR_AMOUNT = 1; + private final static DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); private static final double MOMENT_OF_INERTIA = 0.003; private static final SimpleMotorSimulation RIGHT_MOTOR_SIMULATION = new SimpleMotorSimulation( @@ -61,8 +61,8 @@ public class TransporterConstants { MAXIMUM_DISPLAYABLE_VELOCITY ); - static final double PULSE_VOLTAGE_APPLIED_TIME_SECONDS = 0.1; - static final double PULSE_WAIT_TIME_SECONDS = 0.05; + static final double PULSE_VOLTAGE_APPLIED_TIME_SECONDS = 0.08; + static final double PULSE_WAIT_TIME_SECONDS = 0.03; private static final double HAS_CORAL_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent HAS_CORAL_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), @@ -103,10 +103,10 @@ private static void configureBeamBreak() { public enum TransporterState { REST(0, 0), - COLLECT(5, 5), - ALIGN_CORAL(5, 6), - HOLD_CORAL(1, 1), - EJECT(-5, -5); + COLLECT(-3.5, 5), + ALIGN_CORAL(-5, 6), + HOLD_CORAL(-1, 1), + EJECT(5, -5); public final double targetRightMotorVoltage, diff --git a/src/main/java/lib/hardware/misc/servo/Servo.java b/src/main/java/lib/hardware/misc/servo/Servo.java index 91533c2d..3534ac02 100644 --- a/src/main/java/lib/hardware/misc/servo/Servo.java +++ b/src/main/java/lib/hardware/misc/servo/Servo.java @@ -38,6 +38,10 @@ public void update() { * @param targetSpeed the target speed of the servo, from -1.0 to 1.0. */ public void setTargetSpeed(double targetSpeed) { + if (targetSpeed == 0) { + stop(); + return; + } servoIO.setTargetSpeed(targetSpeed); } @@ -51,9 +55,17 @@ public void setTargetSpeed(double targetSpeed) { * @param value the target position/speed of the servo on a scale from 0 to 1 */ public void set(double value) { + if (value == 0) { + stop(); + return; + } servoIO.set(value); } + public void stop() { + servoIO.stop(); + } + /** * Set the target angle of the servo. * This method is used for angle control servos, where the PWM pulse's width dictates the angle of the servo. diff --git a/src/main/java/lib/hardware/misc/servo/ServoIO.java b/src/main/java/lib/hardware/misc/servo/ServoIO.java index 89a4184f..971c28a7 100644 --- a/src/main/java/lib/hardware/misc/servo/ServoIO.java +++ b/src/main/java/lib/hardware/misc/servo/ServoIO.java @@ -27,6 +27,9 @@ protected void setTargetAngle(Rotation2d targetAngle) { protected void set(double value) { } + protected void stop() { + } + protected void setPWMBoundaries(int maximumPulseWidthMicroseconds, int maximumDeadbandRangeMicroseconds, int centerPulseMicroseconds, int minimumDeadbandRangeMicroseconds, int minimumPulseWidthMicroseconds) { diff --git a/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java b/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java index b6fa616b..b9e47911 100644 --- a/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java +++ b/src/main/java/lib/hardware/misc/servo/io/RealServoIO.java @@ -36,6 +36,11 @@ protected void set(double value) { servo.set(MathUtil.clamp(value, 0, 1)); } + @Override + protected void stop() { + servo.setDisabled(); + } + @Override protected void setPWMBoundaries(int maximumPulseWidthMicroseconds, int maximumDeadbandRangeMicroseconds, int centerPulseMicroseconds, int minimumDeadbandRangeMicroseconds, diff --git a/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java b/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java index cc2cbe31..30081026 100644 --- a/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java +++ b/src/main/java/lib/hardware/misc/servo/io/SimulationServoIO.java @@ -27,6 +27,11 @@ protected void set(double value) { this.targetSpeed = MathUtil.clamp(value, 0, 1); } + @Override + protected void stop() { + this.targetSpeed = 0; + } + @Override protected void setTargetAngle(Rotation2d targetAngle) { final Rotation2d clampedAngle = clampAngleToServoRange(targetAngle);