From 12c84463dc23227ffdec522250a906dca1454651 Mon Sep 17 00:00:00 2001 From: Saikiran Ramanan Date: Tue, 22 Oct 2024 18:41:51 -0700 Subject: [PATCH] Assisted intaking (#68) * create note intake assit manager * rename * intake assist manager inital code * intake assist manager changes * implement intake assist logic into swerveSubsytem & robotManager * remove unused subsystem * fix duplicated code in robotManager Co-authored-by: Jonah Snider * changes * button bindings * fix button bindings * changes * revert back to updateSwerveSpeeds() to finish change tmr & fomat * remove stopIntakingRequest in favor of stowRequest * intake assist changes --------- Co-authored-by: fcuellar13 <105395555+fcuellar13@users.noreply.github.com> Co-authored-by: Jonah Snider --- src/main/java/frc/robot/Robot.java | 12 +++-- src/main/java/frc/robot/imu/ImuSubsystem.java | 9 ++-- .../intake_assist/IntakeAssistManager.java | 51 +++++++++++++++++++ .../localization/LocalizationSubsystem.java | 2 +- .../robot/robot_manager/RobotCommands.java | 7 ++- .../frc/robot/robot_manager/RobotManager.java | 25 ++++++--- .../frc/robot/robot_manager/RobotState.java | 1 + .../java/frc/robot/swerve/SwerveState.java | 4 +- .../frc/robot/swerve/SwerveSubsystem.java | 42 ++++++++++++++- .../util/scheduling/SubsystemPriority.java | 3 +- .../frc/robot/vision/VisionSubsystem.java | 4 +- 11 files changed, 139 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/intake_assist/IntakeAssistManager.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b969f4..b8695cd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ import frc.robot.generated.BuildConstants; import frc.robot.imu.ImuSubsystem; import frc.robot.intake.IntakeSubsystem; +import frc.robot.intake_assist.IntakeAssistManager; import frc.robot.localization.LocalizationSubsystem; import frc.robot.queuer.QueuerSubsystem; import frc.robot.robot_manager.RobotCommands; @@ -39,7 +40,9 @@ public class Robot extends TimedRobot { private final ArmSubsystem arm = new ArmSubsystem(hardware.armLeft, hardware.armRight); private final IntakeSubsystem intake = new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor); - private final SwerveSubsystem swerve = new SwerveSubsystem(); + + private final IntakeAssistManager intakeAssistManager = new IntakeAssistManager(); + private final SwerveSubsystem swerve = new SwerveSubsystem(intakeAssistManager); private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon); private final Limelight leftLimelight = new Limelight("left", InterpolatedVisionDataset.HOME.leftSet); @@ -176,8 +179,11 @@ private void configureBindings() { () -> { robotManager.setConfirmShotActive(false); })); - hardware.driverController.leftTrigger().onTrue(robotCommands.intakeCommand()); - + hardware + .driverController + .leftTrigger() + .onTrue(robotCommands.intakeAssistCommand()) + .onFalse(robotCommands.intakeCommand()); hardware .driverController .rightBumper() diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java index 2862968..82f91e4 100644 --- a/src/main/java/frc/robot/imu/ImuSubsystem.java +++ b/src/main/java/frc/robot/imu/ImuSubsystem.java @@ -2,13 +2,12 @@ import com.ctre.phoenix6.hardware.Pigeon2; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.util.scheduling.SubsystemPriority; import frc.robot.util.state_machines.StateMachine; public class ImuSubsystem extends StateMachine { private final Pigeon2 imu; - private Rotation2d robotHeading = new Rotation2d(); + private static double robotHeading = 0; private double pitch; private double angularVelocity; private double pitchRate; @@ -22,7 +21,7 @@ public ImuSubsystem(Pigeon2 imu) { @Override protected void collectInputs() { - robotHeading = Rotation2d.fromDegrees(imu.getYaw().getValue()); + robotHeading = imu.getYaw().getValue(); angularVelocity = imu.getRate(); pitch = imu.getPitch().getValueAsDouble(); pitchRate = imu.getAngularVelocityYWorld().getValueAsDouble(); @@ -30,7 +29,7 @@ protected void collectInputs() { rollRate = imu.getAngularVelocityXWorld().getValueAsDouble(); } - public Rotation2d getRobotHeading() { + public static double getRobotHeading() { return robotHeading; } @@ -61,7 +60,7 @@ public void setAngle(double zeroAngle) { @Override public void robotPeriodic() { super.robotPeriodic(); - DogLog.log("Imu/RobotHeading", robotHeading.getDegrees()); + DogLog.log("Imu/RobotHeading", robotHeading); DogLog.log("Imu/AngularVelocity", angularVelocity); DogLog.log("Imu/Pitch", pitch); } diff --git a/src/main/java/frc/robot/intake_assist/IntakeAssistManager.java b/src/main/java/frc/robot/intake_assist/IntakeAssistManager.java new file mode 100644 index 0000000..604c147 --- /dev/null +++ b/src/main/java/frc/robot/intake_assist/IntakeAssistManager.java @@ -0,0 +1,51 @@ +package frc.robot.intake_assist; + +import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.imu.ImuSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import frc.robot.vision.LimelightHelpers; + +public class IntakeAssistManager extends LifecycleSubsystem { + private static final double ASSIST_KP = 3.5; + private static final String LIMELIGHT_NAME = "limelight-note"; + private static final double MAX_ANGLE_CHANGE = -35.0; + private static final double MIN_ANGLE_CHANGE = 35.0; + + public IntakeAssistManager() { + super(SubsystemPriority.INTAKE_ASSIST_MANAGER); + } + + public ChassisSpeeds getRobotRelativeAssistSpeeds(ChassisSpeeds fieldRelativeInputSpeeds) { + + double tx = LimelightHelpers.getTX(LIMELIGHT_NAME); + + if (tx == 0) { + return fieldRelativeInputSpeeds; + } + + DogLog.log("IntakeAssist/TX", tx); + + double fieldRelativeNoteAngle = ImuSubsystem.getRobotHeading() + tx; + + double angleError = ImuSubsystem.getRobotHeading() - fieldRelativeNoteAngle; + + double angleChange = MathUtil.clamp(MIN_ANGLE_CHANGE, MAX_ANGLE_CHANGE, angleError * ASSIST_KP); + + Translation2d requestedFieldRelativeDrive = + new Translation2d( + fieldRelativeInputSpeeds.vxMetersPerSecond, fieldRelativeInputSpeeds.vyMetersPerSecond); + + Translation2d newDriveRequest = + requestedFieldRelativeDrive.rotateBy(Rotation2d.fromDegrees(angleChange)); + + return new ChassisSpeeds( + newDriveRequest.getX(), + newDriveRequest.getY(), + fieldRelativeInputSpeeds.omegaRadiansPerSecond); + } +} diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index dadb915..1d71d19 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -39,7 +39,7 @@ public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision, SwerveSub poseEstimator = new SwerveDrivePoseEstimator( SwerveSubsystem.KINEMATICS, - imu.getRobotHeading(), + Rotation2d.fromDegrees(imu.getRobotHeading()), swerve.getModulePositions().toArray(new SwerveModulePosition[4]), new Pose2d()); } diff --git a/src/main/java/frc/robot/robot_manager/RobotCommands.java b/src/main/java/frc/robot/robot_manager/RobotCommands.java index 5638a24..fe619bf 100644 --- a/src/main/java/frc/robot/robot_manager/RobotCommands.java +++ b/src/main/java/frc/robot/robot_manager/RobotCommands.java @@ -20,6 +20,11 @@ public Command intakeCommand() { .andThen(robot.waitForState(RobotState.IDLE_WITH_GP)); } + public Command intakeAssistCommand() { + return Commands.runOnce(robot::intakeAssistRequest, requirements) + .andThen(robot.waitForState(RobotState.IDLE_WITH_GP)); + } + public Command outtakeCommand() { return Commands.runOnce(robot::outtakeRequest, requirements) .andThen(robot.waitForState(RobotState.IDLE_NO_GP)); @@ -46,7 +51,7 @@ public Command idleWithGpCommand() { } public Command stopIntakingCommand() { - return Commands.runOnce(robot::stopIntakingRequest, requirements) + return Commands.runOnce(robot::stowRequest, requirements) .andThen(robot.waitForState(RobotState.IDLE_NO_GP)); } diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 6a8b018..c269eea 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -13,6 +13,7 @@ import frc.robot.shooter.ShooterState; import frc.robot.shooter.ShooterSubsystem; import frc.robot.swerve.SnapUtil; +import frc.robot.swerve.SwerveState; import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.SubsystemPriority; import frc.robot.util.state_machines.StateMachine; @@ -102,7 +103,7 @@ protected RobotState getNextState(RobotState currentState) { shooter.atGoal() && arm.atGoal() ? RobotState.PODIUM_SCORING : currentState; case UNJAM -> currentState; - case INTAKING -> queuer.hasNote() ? RobotState.INTAKING_BACK : currentState; + case INTAKING, INTAKE_ASSIST -> queuer.hasNote() ? RobotState.INTAKING_BACK : currentState; case INTAKING_BACK -> !queuer.hasNote() ? RobotState.INTAKING_FORWARD_PUSH : currentState; }; } @@ -221,6 +222,7 @@ protected void afterTransition(RobotState newState) { intake.setState(IntakeState.INTAKING); queuer.setState(QueuerState.INTAKING); swerve.setSnapsEnabled(false); + swerve.setState(SwerveState.TELEOP); } case INTAKING_BACK -> { arm.setState(ArmState.IDLE); @@ -236,6 +238,17 @@ protected void afterTransition(RobotState newState) { queuer.setState(QueuerState.INTAKING_FORWARD_PUSH); swerve.setSnapsEnabled(false); } + case INTAKE_ASSIST -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.INTAKING); + queuer.setState(QueuerState.INTAKING); + if (DriverStation.isTeleop()) { + swerve.setState(SwerveState.INTAKE_ASSIST_TELEOP); + } else { + swerve.setState(SwerveState.INTAKE_ASSIST_AUTO); + } + } case OUTTAKING -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_STOPPED); @@ -354,15 +367,15 @@ public void intakeRequest() { } } - // TODO: This seems like we ended up not really needing it, can remove it in favor of - // stowRequest() - public void stopIntakingRequest() { + public void intakeAssistRequest() { switch (getState()) { - case INTAKING -> setStateFromRequest(RobotState.IDLE_NO_GP); - default -> {} + case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} + default -> setStateFromRequest(RobotState.INTAKE_ASSIST); } } + // TODO: This seems like we ended up not really needing it, can remove it in favor of + // stowRequest() public void outtakeRequest() { switch (getState()) { case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {} diff --git a/src/main/java/frc/robot/robot_manager/RobotState.java b/src/main/java/frc/robot/robot_manager/RobotState.java index 3d635be..00cb5bf 100644 --- a/src/main/java/frc/robot/robot_manager/RobotState.java +++ b/src/main/java/frc/robot/robot_manager/RobotState.java @@ -20,6 +20,7 @@ public enum RobotState { INTAKING_BACK, INTAKING_FORWARD_PUSH, OUTTAKING, + INTAKE_ASSIST, UNJAM, diff --git a/src/main/java/frc/robot/swerve/SwerveState.java b/src/main/java/frc/robot/swerve/SwerveState.java index 86f195a..4ed4c35 100644 --- a/src/main/java/frc/robot/swerve/SwerveState.java +++ b/src/main/java/frc/robot/swerve/SwerveState.java @@ -4,5 +4,7 @@ public enum SwerveState { TELEOP, TELEOP_SNAPS, AUTO, - AUTO_SNAPS; + AUTO_SNAPS, + INTAKE_ASSIST_TELEOP, + INTAKE_ASSIST_AUTO; } diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java index 17baecf..2e639d4 100644 --- a/src/main/java/frc/robot/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/swerve/SwerveSubsystem.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.config.RobotConfig; import frc.robot.fms.FmsSubsystem; +import frc.robot.intake_assist.IntakeAssistManager; import frc.robot.util.ControllerHelpers; import frc.robot.util.scheduling.SubsystemPriority; import frc.robot.util.state_machines.StateMachine; @@ -55,6 +56,8 @@ public class SwerveSubsystem extends StateMachine { public final Pigeon2 drivetrainPigeon = drivetrain.getPigeon2(); + private final IntakeAssistManager intakeAssistManager; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() // I want field-centric driving in open loop @@ -83,6 +86,10 @@ public class SwerveSubsystem extends StateMachine { private ChassisSpeeds autoSpeeds = new ChassisSpeeds(); + private ChassisSpeeds intakeAssistTeleopSpeeds = new ChassisSpeeds(); + + private ChassisSpeeds intakeAssistAutoSpeeds = new ChassisSpeeds(); + public ChassisSpeeds getRobotRelativeSpeeds() { return robotRelativeSpeeds; } @@ -107,12 +114,13 @@ public void setSnapToAngle(double angle) { goalSnapAngle = angle; } - public SwerveSubsystem() { + public SwerveSubsystem(IntakeAssistManager intakeAssistManager) { super(SubsystemPriority.SWERVE, SwerveState.TELEOP); driveToAngle.HeadingController = RobotConfig.get().swerve().snapController(); driveToAngle.HeadingController.enableContinuousInput(-Math.PI, Math.PI); driveToAngle.HeadingController.setTolerance(0.02); modulePositions = calculateModulePositions(); + this.intakeAssistManager = intakeAssistManager; } public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) { @@ -120,6 +128,16 @@ public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) { sendSwerveRequest(); } + public void setIntakeAssistTeleopSpeeds(ChassisSpeeds speeds) { + intakeAssistTeleopSpeeds = speeds; + sendSwerveRequest(); + } + + public void setIntakeAssistAutoSpeeds(ChassisSpeeds speeds) { + intakeAssistAutoSpeeds = speeds; + sendSwerveRequest(); + } + public void driveTeleop(double x, double y, double theta) { double leftY = -1.0 @@ -162,6 +180,7 @@ protected void collectInputs() { fieldRelativeSpeeds = calculateFieldRelativeSpeeds(); slowEnoughToShoot = calculateMovingSlowEnoughForSpeakerShot(robotRelativeSpeeds); slowEnoughToFeed = calculateMovingSlowEnoughForFloorShot(robotRelativeSpeeds); + } private List calculateModulePositions() { @@ -212,6 +231,7 @@ private void sendSwerveRequest() { .withVelocityY(teleopSpeeds.vyMetersPerSecond) .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle)) .withDriveRequestType(DriveRequestType.OpenLoopVoltage)); + } else { drivetrain.setControl( drive @@ -221,6 +241,16 @@ private void sendSwerveRequest() { .withDriveRequestType(DriveRequestType.OpenLoopVoltage)); } } + case INTAKE_ASSIST_TELEOP -> { + intakeAssistTeleopSpeeds = intakeAssistManager.getRobotRelativeAssistSpeeds(teleopSpeeds); + + drivetrain.setControl( + drive + .withVelocityX(intakeAssistTeleopSpeeds.vxMetersPerSecond) + .withVelocityY(intakeAssistTeleopSpeeds.vyMetersPerSecond) + .withRotationalRate(intakeAssistTeleopSpeeds.omegaRadiansPerSecond) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage)); + } case AUTO -> drivetrain.setControl( drive @@ -235,8 +265,18 @@ private void sendSwerveRequest() { .withVelocityY(autoSpeeds.vyMetersPerSecond) .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle)) .withDriveRequestType(DriveRequestType.Velocity)); + + case INTAKE_ASSIST_AUTO -> { + intakeAssistAutoSpeeds = intakeAssistManager.getRobotRelativeAssistSpeeds(autoSpeeds); + drivetrain.setControl( + drive + .withVelocityX(intakeAssistAutoSpeeds.vxMetersPerSecond) + .withVelocityY(intakeAssistAutoSpeeds.vyMetersPerSecond) + .withRotationalRate(intakeAssistAutoSpeeds.omegaRadiansPerSecond) + .withDriveRequestType(DriveRequestType.Velocity)); } } + } public void setState(SwerveState newState) { setStateFromRequest(newState); diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java index b7d1620..ce569d1 100644 --- a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java +++ b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java @@ -23,7 +23,8 @@ public enum SubsystemPriority { // Robot manager runs last so that all sensor data is fresh before processing state transitions ROBOT_MANAGER(0), - AUTOS(0); + AUTOS(0), + INTAKE_ASSIST_MANAGER(0); final int value; diff --git a/src/main/java/frc/robot/vision/VisionSubsystem.java b/src/main/java/frc/robot/vision/VisionSubsystem.java index 837723c..63d7ac5 100644 --- a/src/main/java/frc/robot/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/vision/VisionSubsystem.java @@ -37,8 +37,8 @@ public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight righ @Override protected void collectInputs() { - robotHeading = imu.getRobotHeading().getDegrees(); - DogLog.log("Vision/RobotHeading", imu.getRobotHeading().getDegrees()); + robotHeading = imu.getRobotHeading(); + DogLog.log("Vision/RobotHeading", imu.getRobotHeading()); angularVelocity = imu.getRobotAngularVelocity(); pitch = imu.getPitch(); pitchRate = imu.getPitchRate();