From d1421eddba0e6d1292888a21d62e9f2050d22125 Mon Sep 17 00:00:00 2001 From: Shayaan Wadkar <82843611+Shom770@users.noreply.github.com> Date: Wed, 17 Apr 2024 03:57:36 -0400 Subject: [PATCH] Wrist tuning (#46) * working battery voltage status bar * more led stuff * more led stuff * fix feeder and LL * finish leds * spotless * elevator real * resolved merge conflicts * theta stuff * more merge conflicts * new stuff * tree map tuning * tuned shooter * tuned wrist * add high interpolating map * auto fixes and high note tree map * load in changes * add aiming leds * add passing shot and day 1 changes * y * fix amp auto and add second passsing shot * hopefully fix intake handoff issues * EOL for drive * add autonomous check for extra grount intake transitions * tuned wrist * wedsday robot work * revert testing changes * force idle when auto aim release * pre worlds auto changes * fix build --------- Co-authored-by: Matthew Choulas Co-authored-by: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> --- .../com/team4099/utils/LimelightHelpers.java | 228 +++++++++++- .../CustomHolonomicDriveController.kt | 3 +- .../kotlin/com/team4099/robot2023/Robot.kt | 15 +- .../com/team4099/robot2023/RobotContainer.kt | 74 ++-- .../robot2023/auto/AutonomousSelector.kt | 18 +- .../robot2023/auto/mode/FiveNoteAutoPath.kt | 10 +- .../robot2023/auto/mode/FourNoteAutoPath.kt | 55 ++- .../robot2023/auto/mode/TestAutoPath.kt | 8 +- ...eeNoteAndPickupCenterlineSourceAutoPath.kt | 313 ++++++++++------ .../ThreeNoteCenterlineFromAmpAutoPath.kt | 339 +++++++++++------- .../commands/drivetrain/DrivePathCommand.kt | 14 +- .../commands/drivetrain/LockDriveCommand.kt | 17 + .../commands/drivetrain/TargetNoteCommand.kt | 44 ++- .../drivetrain/TargetSpeakerCommand.kt | 9 +- .../team4099/robot2023/config/ControlBoard.kt | 5 +- .../robot2023/config/constants/Constants.kt | 5 +- .../config/constants/DrivetrainConstants.kt | 34 +- .../config/constants/ElevatorConstants.kt | 3 +- .../config/constants/FlywheelConstants.kt | 14 +- .../config/constants/IntakeConstants.kt | 11 +- .../config/constants/LEDConstants.kt | 20 +- .../constants/SuperstructureConstants.kt | 84 +++-- .../config/constants/VisionConstants.kt | 6 +- .../config/constants/WristConstants.kt | 39 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 11 +- .../drivetrain/swervemodule/SwerveModule.kt | 4 +- .../drivetrain/swervemodule/SwerveModuleIO.kt | 2 +- .../swervemodule/SwerveModuleIOSim.kt | 2 +- .../swervemodule/SwerveModuleIOTalon.kt | 3 +- .../robot2023/subsystems/elevator/Elevator.kt | 14 +- .../subsystems/elevator/ElevatorIO.kt | 2 +- .../robot2023/subsystems/flywheel/Flywheel.kt | 28 +- .../subsystems/flywheel/FlywheelIO.kt | 2 +- .../subsystems/flywheel/FlywheelIOTalon.kt | 58 +-- .../robot2023/subsystems/intake/IntakeIO.kt | 2 +- .../subsystems/intake/IntakeIOFalconNEO.kt | 18 +- .../subsystems/intake/IntakeIOSim.kt | 2 +- .../robot2023/subsystems/led/LedIO.kt | 3 + .../robot2023/subsystems/led/LedIOCandle.kt | 179 ++++++++- .../team4099/robot2023/subsystems/led/Leds.kt | 48 ++- .../subsystems/limelight/LimelightVision.kt | 9 + .../subsystems/superstructure/AutoAim.kt | 115 ++++-- .../subsystems/superstructure/Request.kt | 2 + .../superstructure/Superstructure.kt | 330 ++++++++++++----- .../robot2023/subsystems/vision/Vision.kt | 26 +- .../robot2023/subsystems/wrist/Wrist.kt | 7 + 46 files changed, 1621 insertions(+), 614 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt diff --git a/src/main/java/com/team4099/utils/LimelightHelpers.java b/src/main/java/com/team4099/utils/LimelightHelpers.java index 0cb199a3..1431ef51 100644 --- a/src/main/java/com/team4099/utils/LimelightHelpers.java +++ b/src/main/java/com/team4099/utils/LimelightHelpers.java @@ -1,4 +1,5 @@ -package com.team4099.utils;//com.team4099.utils.LimelightHelpers v1.2.1 (March 1, 2023) +package com.team4099.utils;//com.team4099.utils.LimelightHelpers +//LimelightHelpers v1.5.0 (March 27, 2024) import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -301,6 +302,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -361,8 +374,59 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -383,7 +447,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -395,7 +459,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -403,6 +467,86 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + // System.out.println("No PoseEstimate available."); + return; + } + +// System.out.printf("Pose Estimate Information:%n"); +// System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); +// System.out.printf("Latency: %.3f ms%n", pose.latency); +// System.out.printf("Tag Count: %d%n", pose.tagCount); +// System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); +// System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); +// System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); +// System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -542,8 +686,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -602,6 +746,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -616,6 +782,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -641,6 +827,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -694,6 +885,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -763,7 +976,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); @@ -775,5 +988,4 @@ public static LimelightResults getLatestResults(String limelightName) { return results; } -} - +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt index b5d45d2d..6f7f8689 100644 --- a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt +++ b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt @@ -1,7 +1,6 @@ package com.team4099.lib.trajectory import edu.wpi.first.math.controller.PIDController -import edu.wpi.first.math.controller.ProfiledPIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,7 +21,7 @@ import org.team4099.lib.units.derived.radians class CustomHolonomicDriveController( private val m_xController: PIDController, private val m_yController: PIDController, - private val m_thetaController: ProfiledPIDController + private val m_thetaController: PIDController ) { private var m_poseError = Pose2d() private var m_rotationError = Rotation2d() diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 0066d232..ea1281cd 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -22,7 +22,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.simulation.DriverStationSim import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler -import edu.wpi.first.wpilibj2.command.WaitCommand +import edu.wpi.first.wpilibj2.command.Commands.runOnce import org.ejml.EjmlVersion.BUILD_DATE import org.ejml.EjmlVersion.DIRTY import org.ejml.EjmlVersion.GIT_BRANCH @@ -128,15 +128,15 @@ object Robot : LoggedRobot() { // Set the scheduler to log events for command initialize, interrupt, finish CommandScheduler.getInstance().onCommandInitialize { command: Command -> - DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", true) + Logger.recordOutput("/ActiveCommands/${command.name}", true) } CommandScheduler.getInstance().onCommandFinish { command: Command -> - DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false) + Logger.recordOutput("/ActiveCommands/${command.name}", false) } CommandScheduler.getInstance().onCommandInterrupt { command: Command -> - DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false) + Logger.recordOutput("/ActiveCommands/${command.name}", false) } val autoTab = Shuffleboard.getTab("Pre-match") @@ -146,12 +146,13 @@ object Robot : LoggedRobot() { .withPosition(0, 1) .withWidget(BuiltInWidgets.kTextView) .entry - - RobotContainer.zeroSensors(isInAutonomous = true) } override fun autonomousInit() { - val autonCommandWithWait = WaitCommand(0.001).andThen(autonomousCommand) + RobotContainer.setSteeringCoastMode() + + val autonCommandWithWait = + runOnce({ RobotContainer.zeroSensors(isInAutonomous = true) }).andThen(autonomousCommand) autonCommandWithWait?.schedule() } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9c96230a..6b8fbe4a 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023 import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.auto.AutonomousSelector +import com.team4099.robot2023.commands.drivetrain.LockDriveCommand import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand @@ -28,6 +29,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIOFalconNEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO +import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision @@ -39,6 +41,7 @@ import com.team4099.robot2023.subsystems.wrist.WristIOTalon import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands.runOnce import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import org.team4099.lib.smoothDeadband @@ -77,7 +80,7 @@ object RobotContainer { drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2")) - limelight = LimelightVision(object : LimelightVisionIO {}) + limelight = LimelightVision(LimelightVisionIOReal) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) elevator = Elevator(ElevatorIONEO) @@ -86,7 +89,7 @@ object RobotContainer { } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2")) + vision = Vision(object : CameraIO {}) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOSim) feeder = Feeder(FeederIOSim) @@ -95,7 +98,8 @@ object RobotContainer { wrist = Wrist(WristIOSim) } - superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision) + superstructure = + Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision, limelight) vision.setDataInterfaces( { drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }, @@ -117,6 +121,8 @@ object RobotContainer { drivetrain ) + superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand() + /* module steeing tuning @@ -161,6 +167,7 @@ object RobotContainer { } fun mapTeleopControls() { + limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) ControlBoard.intake.whileTrue( @@ -173,13 +180,36 @@ object RobotContainer { { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain, - limelight + limelight, + feeder ) ) ) ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand()) + ControlBoard.passingShotAlignment.whileTrue( + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + { + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red + ) + 225.degrees + 180.degrees + else if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue + ) + 135.degrees + else 135.degrees + }, + ) + ) + ControlBoard.prepHighProtected.whileTrue( ParallelCommandGroup( superstructure.prepSpeakerMidCommand(), @@ -206,31 +236,20 @@ object RobotContainer { ControlBoard.retractClimb.whileTrue(superstructure.climbRetractCommand()) ControlBoard.forceIdle.whileTrue(superstructure.requestIdleCommand()) ControlBoard.prepLow.whileTrue(superstructure.prepSpeakerLowCommand()) - ControlBoard.prepTrap.whileTrue(superstructure.prepTrapCommand()) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand()) + ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand()) ControlBoard.targetAmp.whileTrue( - runOnce({ - val currentRotation = drivetrain.odomTRobot.rotation - setAmpAngle = - if (currentRotation > 0.0.degrees && currentRotation < 180.degrees) { - 90.degrees - } else { - 270.degrees - } - }) - .andThen( - TargetAngleCommand( - driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, - { ControlBoard.slowMode }, - drivetrain, - ampAngle - ) - ) + TargetAngleCommand( + driver = Ryan(), + { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { ControlBoard.slowMode }, + drivetrain, + ampAngle + ) ) ControlBoard.climbAlignFar.whileTrue( @@ -317,12 +336,15 @@ object RobotContainer { { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain, + superstructure, vision ), superstructure.autoAimCommand() ) ) + ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain)) + // ControlBoard.characterizeSubsystem.whileTrue( // WheelRadiusCharacterizationCommand( // drivetrain, WheelRadiusCharacterizationCommand.Companion.Direction.CLOCKWISE @@ -359,5 +381,7 @@ object RobotContainer { fun getAutonomousLoadingCommand() = AutonomousSelector.getLoadingCommand(drivetrain) + fun resetGyroYawCommand(angle: Angle): Command = ResetGyroYawCommand(drivetrain, angle) + fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index 9a113d6b..5c11ff5c 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -46,6 +46,8 @@ object AutonomousSelector { autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH) + /* + autonomousModeChooser.addOption( "Four Note Right Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH ) @@ -55,9 +57,14 @@ object AutonomousSelector { autonomousModeChooser.addOption( "Four Note LEFT Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_LEFT_AUTO_PATH ) + + */ + autonomousModeChooser.addOption( "Five Note Auto from Center Subwoofer", AutonomousMode.FIVE_NOTE_AUTO_PATH ) + + /* autonomousModeChooser.addOption( "Two Note Centerline Auto from Source Side of Subwoofer", AutonomousMode.TWO_NOTE_CENTERLINE_FROM_SOURCE @@ -67,8 +74,11 @@ object AutonomousSelector { "Two Note Centerline Auto from Amp Side of Subwoofer", AutonomousMode.TWO_NOTE_CENTERLINE_FROM_AMP ) + + */ + autonomousModeChooser.addOption( - "Three Note Centerline Auto from Amp Side of Subwoofer", + "Three Note + Pickup Centerline Auto from Amp Side of Subwoofer", AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP ) autonomousModeChooser.addOption( @@ -88,10 +98,14 @@ object AutonomousSelector { AutonomousMode.PRELOAD_AND_LEAVE_CENTER_SUBWOOFER ) + /* + autonomousModeChooser.addOption("Six Note Path", AutonomousMode.SIX_NOTE_AUTO_PATH) autonomousModeChooser.addOption( "Six Note Path with Pickup", AutonomousMode.SIX_NOTE_WITH_PICKUP_PATH ) + + */ autonomousModeChooser.addOption("Test Auto Path", AutonomousMode.TEST_AUTO_PATH) // autonomousModeChooser.addOption("Characterize Elevator", // AutonomousMode.ELEVATOR_CHARACTERIZE) @@ -114,7 +128,7 @@ object AutonomousSelector { } val waitTime: Time - get() = waitBeforeCommandSlider.getDouble(0.0).seconds + get() = 0.0.seconds val secondaryWaitTime: Time get() = secondaryWaitInAuto.getDouble(0.0).seconds diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt index fadcd1c8..47dc8d7e 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt @@ -43,7 +43,7 @@ class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru ) ), superstructure.prepManualSpeakerCommand( - -3.7.degrees, 3000.rotations.perMinute, 0.7.degrees + -4.5.degrees, 3000.rotations.perMinute, 0.6.degrees ), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -86,10 +86,11 @@ class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru .andThen(WaitCommand(0.25)) .andThen( superstructure.prepManualSpeakerCommand( - -3.7.degrees, 3000.rotations.perMinute, 0.7.degrees + -4.5.degrees, 3000.rotations.perMinute, 0.6.degrees ) ) ), + superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -114,8 +115,7 @@ class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ), superstructure - .scoreCommand() - .andThen(superstructure.groundIntakeCommand()) + .groundIntakeCommand() .andThen( superstructure.prepManualSpeakerCommand( -3.degrees, 3000.rotations.perMinute, 0.7.degrees @@ -150,7 +150,7 @@ class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru .andThen(WaitCommand(0.1)) .andThen(superstructure.groundIntakeCommand()) ), - superstructure.prepManualSpeakerCommand(-3.degrees, 3000.rotations.perMinute, 0.7.degrees), + superstructure.prepManualSpeakerCommand(-3.degrees, 3000.rotations.perMinute), superstructure.scoreCommand().withTimeout(0.5) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt index 5a6772f5..f52427eb 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt @@ -23,7 +23,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru addCommands( superstructure.prepSpeakerLowCommand(), superstructure.scoreCommand().withTimeout(0.5), - WaitCommand(0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -54,12 +54,14 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.75).andThen(superstructure.groundIntakeCommand()) + WaitCommand(0.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(1.75)) + .andThen(superstructure.prepSpeakerLowCommand()) ), - superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() - .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.3), + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -90,12 +92,14 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.5).andThen(superstructure.groundIntakeCommand()) + WaitCommand(0.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(1.75)) + .andThen(superstructure.prepSpeakerLowCommand()) ), - superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() - .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.3), + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -142,12 +146,43 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru } ) .withTimeout(3.235 + 0.5), - WaitCommand(0.3).andThen(superstructure.groundIntakeCommand()) + WaitCommand(0.3) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(1.75)) + .andThen(superstructure.prepSpeakerLowCommand()) ), - superstructure.prepSpeakerLowCommand(), superstructure .scoreCommand() - .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.3) + .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5), + WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + startingPose.translation.translation2d, null, 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.45.meters, 4.89.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.92.meters, 4.0.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.29.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + } + ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + ) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 9f5e456b..f28d232d 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -27,18 +27,18 @@ class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructur startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 2.meters, startingPose.y + 0.02.meters) + Translation2d(startingPose.x + 1.5.meters, startingPose.y + 0.02.meters) .translation2d, null, (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 4.meters, startingPose.y).translation2d, + Translation2d(startingPose.x + 3.meters, startingPose.y).translation2d, null, (startingPose.rotation + 45.degrees).inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 2.meters, startingPose.y - 0.02.meters) + Translation2d(startingPose.x + 1.5.meters, startingPose.y - 0.02.meters) .translation2d, null, (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds @@ -50,7 +50,7 @@ class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructur ) ) }, - useLowerTolerance = false + useLowerTolerance = true ) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt index 6beba4c7..b3c255b9 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -4,12 +4,12 @@ import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ConditionalCommand import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Translation2d -import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds @@ -23,6 +23,8 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( init { addRequirements(drivetrain) addCommands( + superstructure.prepSpeakerLowCommand(), + superstructure.scoreCommand().withTimeout(0.75), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -30,149 +32,234 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( listOf( FieldWaypoint( startingPose.translation.translation2d, - 0.degrees.inRotation2ds, + null, startingPose.rotation.inRotation2ds ), FieldWaypoint( - startingPose.translation.translation2d + - Translation2d(2.inches, -2.inches).translation2d, - 0.degrees.inRotation2ds, - (startingPose.rotation - 47.546.degrees).inRotation2ds - ) - ) - } - ), - superstructure.prepManualSpeakerCommand(-20.degrees, 3000.rotations.perMinute) - ), - superstructure.scoreCommand(), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - startingPose.translation.translation2d + - Translation2d(2.inches, -2.inches).translation2d, + Translation2d(4.33.meters, 1.67.meters).translation2d, null, - (startingPose.rotation - 47.546.degrees).inRotation2ds + 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, + Translation2d(8.32.meters, 0.78.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(8.29.meters, 0.78.meters).translation2d, + Translation2d(6.89.meters, 1.73.meters).translation2d, null, 180.degrees.inRotation2ds ) ) } ), - WaitCommand(2.0).andThen(superstructure.groundIntakeCommand()) + WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.29.meters, 0.78.meters).translation2d, - null, - 180.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.37583640633171).degrees.inRotation2ds + ConditionalCommand( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(6.89.meters, 1.73.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.37583640633171).degrees.inRotation2ds + ), + ) + } + ), + superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute) + ) + .andThen(superstructure.scoreCommand()) + .andThen( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (4.33.meters + 8.32.meters) / 2, + (2.44 + 0.78).meters / 2 - 0.1.meters + ) + .translation2d, + null, + ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.0.meters, 2.40.meters).translation2d, + null, + 220.degrees.inRotation2ds + ) + ) + } ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()), ) - } + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(6.89.meters, 1.73.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.0.meters, 2.40.meters).translation2d, + null, + 220.degrees.inRotation2ds + ) + ) + } + ), + superstructure.groundIntakeCommand() ), - WaitCommand(1.0) - .andThen( - superstructure.prepManualSpeakerCommand(5.degrees, 4000.rotations.perMinute) - ) + { superstructure.holdingNote } ), - superstructure.scoreCommand(), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.29.meters) / 2, - (2.44 + 0.78).meters / 2 - 0.1.meters + ConditionalCommand( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(7.0.meters, 2.40.meters).translation2d, + null, + 220.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (4.33.meters + 8.32.meters) / 2, + (2.44 + 0.78).meters / 2 + 0.1.meters + ) + .translation2d, + null, + ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds, + ), + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds ) - .translation2d, - null, - ((180 - 43.3758 + 210) / 2 + 10).degrees.inRotation2ds, - ), - FieldWaypoint( - Translation2d(8.29.meters, 2.44.meters).translation2d, - null, - 210.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.29.meters) / 2, - (2.44 + 0.78).meters / 2 + 0.1.meters - ) - .translation2d, - null, - ((180 - 43.3758 + 210) / 2 + 10).degrees.inRotation2ds, - ), - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds ) - ) - } - ), - WaitCommand(1.0) - .andThen(superstructure.groundIntakeCommand()) - .andThen(WaitCommand(0.5)) + } + ), + superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute) + ) + .andThen(superstructure.scoreCommand()) .andThen( - superstructure.prepManualSpeakerCommand(5.degrees, 4000.rotations.perMinute) - ) - ), - superstructure.scoreCommand(), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d(4.84.meters, 4.09.meters).translation2d, - null, - 180.degrees.inRotation2ds + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(4.33.meters, 1.67.meters).translation2d, + null, + (180 - 43.3758).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.84.meters, 4.09.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.2.meters, 4.06.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.5.meters, 2.0.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.5.meters, 1.85.meters).translation2d, + null, + (180 - 35.02902884839783).degrees.inRotation2ds + ) + ) + } ), - FieldWaypoint( - Translation2d(8.29.meters, 4.12.meters).translation2d, - null, - 180.degrees.inRotation2ds + WaitCommand(1.5) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -2.0.degrees, 4400.rotations.perMinute + ) + ) + ) + .andThen(superstructure.scoreCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(7.0.meters, 2.40.meters).translation2d, + null, + 220.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.2.meters, 4.06.meters).translation2d, + null, + 220.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.5.meters, 2.0.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.5.meters, 1.85.meters).translation2d, + null, + (180 - 35.02902884839783).degrees.inRotation2ds + ) + ) + } + ), + superstructure + .groundIntakeCommand() + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -2.0.degrees, 4400.rotations.perMinute ) ) - } - ), - WaitCommand(1.5).andThen(superstructure.groundIntakeCommand()) + ) + .andThen(superstructure.scoreCommand()), + { superstructure.holdingNote } ) ) } companion object { - val startingPose = Pose2d(1.40.meters, 4.09.meters, 180.degrees) + val startingPose = Pose2d(0.63.meters, 4.44.meters, 120.degrees) } } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt index 18f5fc59..cc1b75c0 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -4,11 +4,13 @@ import com.team4099.lib.trajectory.FieldWaypoint import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Superstructure +import edu.wpi.first.wpilibj2.command.ConditionalCommand import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.WaitCommand import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRotation2ds @@ -17,13 +19,14 @@ import org.team4099.lib.units.perMinute class ThreeNoteCenterlineFromAmpAutoPath( val drivetrain: Drivetrain, - val superstructure: Superstructure + val superstructure: Superstructure, ) : SequentialCommandGroup() { + init { addRequirements(drivetrain) addCommands( superstructure.prepSpeakerLowCommand(), - superstructure.scoreCommand(), + superstructure.scoreCommand().withTimeout(0.75), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, @@ -45,155 +48,225 @@ class ThreeNoteCenterlineFromAmpAutoPath( 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(8.27.meters, 7.45.meters).translation2d, - null, - 180.degrees.inRotation2ds - ) - ) - } - ), - WaitCommand(2.0).andThen(superstructure.groundIntakeCommand()) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.27.meters, 7.45.meters).translation2d, + Translation2d(8.27.meters + 2.feet, 7.45.meters).translation2d, null, 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, + Translation2d(7.0.meters, 6.65.meters).translation2d, null, - (180 + 13.856).degrees.inRotation2ds - ), - ) - } - ), - WaitCommand(1.0) - .andThen( - superstructure - .prepManualSpeakerCommand( - 8.870702276919971.degrees, 4000.rotations.perMinute + 180.degrees.inRotation2ds ) - .withTimeout(1.0) - ) - ), - superstructure.scoreCommand().withTimeout(0.5), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) - .translation2d, - null, - ((180 + 13.856 + 165).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), ) } ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + WaitCommand(2.25).andThen(superstructure.groundIntakeCommand()) ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.27.meters, 5.78.meters).translation2d, - null, - 160.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) - .translation2d, - null, - ((180 + 13.856 + 160).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), + ConditionalCommand( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(7.0.meters, 6.65.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + WaitCommand(1.0) + .andThen( + superstructure + .prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) + .withTimeout(1.0) ) - } - ), - WaitCommand(1.0) + ) + .andThen(superstructure.scoreCommand().withTimeout(0.5)) .andThen( - superstructure.prepManualSpeakerCommand(2.degrees, 4000.rotations.perMinute) - ) - ), - superstructure.scoreCommand().withTimeout(0.5), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) - .translation2d, - null, - ((180 + 13.856 + 165).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(8.27.meters, 4.11.meters).translation2d, - null, - 140.degrees.inRotation2ds + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2 + ) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 5.78.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.0.meters, 5.7.meters).translation2d, + null, + 160.degrees.inRotation2ds + ) + ) + } ), + WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) ) - } + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(7.0.meters, 6.65.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 5.78.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.0.meters, 5.7.meters).translation2d, + null, + 160.degrees.inRotation2ds + ) + ) + } + ), + superstructure.groundIntakeCommand() ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()) + { superstructure.holdingNote } ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.27.meters, 4.11.meters).translation2d, - null, - 140.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d((3.9 + 8.27).meters / 2, (6.45 + 7.45).meters / 2) - .translation2d, - null, - ((180 + 13.856 + 160).degrees / 2).inRotation2ds - ), - FieldWaypoint( - Translation2d(3.9.meters, 6.45.meters).translation2d, - null, - (180 + 13.856).degrees.inRotation2ds + ConditionalCommand( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(7.0.meters, 5.7.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d((3.9 + 8.27).meters / 2, (5.7 + 7.45).meters / 2) + .translation2d, + null, + ((180 + 13.856 + 160).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + ) + } + ), + superstructure.prepManualSpeakerCommand(-7.degrees, 4000.rotations.perMinute) + ) + .andThen(superstructure.scoreCommand().withTimeout(0.5)) + .andThen( + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(3.9.meters, 6.45.meters).translation2d, + null, + (180 + 13.856).degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d( + (3.9 + 8.27).meters / 2, (5.7 + 7.45).meters / 2 + ) + .translation2d, + null, + ((180 + 13.856 + 165).degrees / 2).inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.5.meters, 2.0.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.5.meters, 1.85.meters).translation2d, + null, + (180 - 35.02902884839783).degrees.inRotation2ds + ) + ) + } ), + WaitCommand(1.0) + .andThen(superstructure.groundIntakeCommand()) + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -2.0.degrees, 4400.rotations.perMinute + ) + ) ) - } - ), - WaitCommand(1.0) - .andThen( - superstructure.prepManualSpeakerCommand(2.degrees, 4000.rotations.perMinute) - ) - ), - superstructure.scoreCommand() + .andThen(superstructure.scoreCommand()) + ), + ParallelCommandGroup( + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(7.0.meters, 5.7.meters).translation2d, + null, + 160.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(8.27.meters, 4.11.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.5.meters, 2.0.meters).translation2d, + null, + 140.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(5.5.meters, 1.85.meters).translation2d, + null, + (180 - 35.02902884839783).degrees.inRotation2ds + ) + ) + } + ), + superstructure + .groundIntakeCommand() + .andThen(WaitCommand(2.0)) + .andThen( + superstructure.prepManualSpeakerCommand( + -2.0.degrees, 4400.rotations.perMinute + ) + ) + ) + .andThen(superstructure.scoreCommand()), + { superstructure.holdingNote } + ) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 8fcf16bf..7549882c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -24,8 +24,6 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController -import org.team4099.lib.controller.ProfiledPIDController -import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Transform2d @@ -83,7 +81,7 @@ private constructor( private val xPID: PIDController> private val yPID: PIDController> - private val thetaPID: ProfiledPIDController> + private val thetaPID: PIDController> private val swerveDriveController: CustomHolonomicDriveController @@ -194,13 +192,7 @@ private constructor( xPID = PIDController(poskP.get(), poskI.get(), poskD.get()) yPID = PIDController(poskP.get(), poskI.get(), poskD.get()) - thetaPID = - ProfiledPIDController( - thetakP.get(), - thetakI.get(), - thetakD.get(), - TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) - ) + thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) thetaPID.enableContinuousInput(-PI.radians, PI.radians) @@ -246,7 +238,7 @@ private constructor( // drivetrain.odometryPose = AllianceFlipUtil.apply(Pose2d(trajectory.initialPose)) // } trajStartTime = Clock.fpgaTime + trajectory.states[0].timeSeconds.seconds - thetaPID.reset(0.degrees) + thetaPID.reset() xPID.reset() yPID.reset() } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt new file mode 100644 index 00000000..03a04ff5 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/LockDriveCommand.kt @@ -0,0 +1,17 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj2.command.Command + +class LockDriveCommand(val drivetrain: Drivetrain) : Command() { + + override fun execute() { + drivetrain.defaultCommand.end(true) + drivetrain.currentRequest = Request.DrivetrainRequest.LockWheels() + } + + override fun isFinished(): Boolean { + return false + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index 4dadbafc..b83fcdd6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -3,14 +3,18 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.driver.DriverProfile +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -22,8 +26,10 @@ import org.team4099.lib.units.derived.perDegreePerSecond import org.team4099.lib.units.derived.perDegreeSeconds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import kotlin.math.PI +import kotlin.math.hypot class TargetNoteCommand( val driver: DriverProfile, @@ -32,7 +38,8 @@ class TargetNoteCommand( val turn: () -> Double, val slowMode: () -> Boolean, val drivetrain: Drivetrain, - val limelight: LimelightVision + val limelight: LimelightVision, + val feeder: Feeder ) : Command() { private var thetaPID: PIDController> @@ -98,26 +105,45 @@ class TargetNoteCommand( override fun initialize() { thetaPID.reset() + /* if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) } + + */ } override fun execute() { drivetrain.defaultCommand.end(true) - DebugLogger.recordDebugOutput("ActiveCommands/TargetNoteCommand", true) + Logger.recordOutput("ActiveCommands/TargetNoteCommand", true) val thetaFeedback = thetaPID.calculate(limelight.targetGamePieceTx ?: 0.0.degrees, 0.0.degrees) DebugLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) DebugLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - thetaFeedback, - driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), - fieldOriented = false - ) + if (!feeder.hasNote && limelight.inputs.gamePieceTargets.size > 0) { + val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) + var autoDriveVector = + hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + if (DriverStation.getAlliance().isPresent && + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue + ) { + autoDriveVector = + -hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + } + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + Pair(autoDriveVector.meters.perSecond, 0.0.meters.perSecond), + fieldOriented = false + ) + } else { + val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) + val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) + + drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop(rotation, speed) + } } override fun isFinished(): Boolean { @@ -125,7 +151,7 @@ class TargetNoteCommand( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) + Logger.recordOutput("ActiveCommands/TargetAngleCommand", false) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( driver.rotationSpeedClampedSupplier(turn, slowMode), diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt index 7c125f42..31b9d0da 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt @@ -5,6 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.util.driver.DriverProfile import com.team4099.robot2023.util.inverse @@ -15,6 +16,7 @@ import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -38,6 +40,7 @@ class TargetSpeakerCommand( val turn: () -> Double, val slowMode: () -> Boolean, val drivetrain: Drivetrain, + val superstructure: Superstructure, val vision: Vision ) : Command() { private var thetaPID: PIDController> @@ -121,6 +124,7 @@ class TargetSpeakerCommand( } drivetrain.defaultCommand.end(true) + superstructure.defaultCommand.end(true) Logger.recordOutput("ActiveCommands/TargetAngleCommand", true) Logger.recordOutput( "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees @@ -135,13 +139,14 @@ class TargetSpeakerCommand( robotTSpeaker.y.inMeters - ( drivetrain.robotVelocity.y * - robotTSpeaker.translation.magnitude.absoluteValue / 5 + robotTSpeaker.translation.magnitude.absoluteValue / 7 ) .value, robotTSpeaker.x.inMeters - + 10.inches.inMeters - ( drivetrain.robotVelocity.x * - robotTSpeaker.translation.magnitude.absoluteValue / 5 + robotTSpeaker.translation.magnitude.absoluteValue / 7 ) .value ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 4ec9936f..89c0b871 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -54,11 +54,12 @@ object ControlBoard { val prepHighProtected = Trigger { operator.bButton } val prepHigh = Trigger { operator.yButton } val passingShot = Trigger { operator.leftShoulderButton } + val passingShotAlignment = Trigger { driver.yButton } + val underStagePassingShot = Trigger { operator.rightShoulderButton } val extendClimb = Trigger { operator.dPadUp } val retractClimb = Trigger { operator.dPadDown } - val prepTrap = Trigger { operator.rightShoulderButton } val ejectGamePiece = Trigger { driver.rightTriggerAxis > 0.5 } val testWrist = Trigger { driver.aButton } @@ -72,5 +73,7 @@ object ControlBoard { val targetSpeaker = Trigger { driver.xButton } // TODO: switch back to climbAlignLeft val climbAutoAlign = Trigger { driver.bButton } + val lockWheels = Trigger { driver.selectButton } + // week0 controls } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 41089e90..f6427e2e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -53,9 +53,8 @@ object Constants { } object Tuning { - - const val TUNING_MODE = false - const val DEBUGING_MODE = false + const val TUNING_MODE = true + const val DEBUGING_MODE = true const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 41ec3a85..21fab9c7 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -33,7 +33,7 @@ object DrivetrainConstants { const val FOC_ENABLED = true const val MINIMIZE_SKEW = false - const val TELEOP_TURNING_SPEED_PERCENT = 0.75 + const val TELEOP_TURNING_SPEED_PERCENT = 0.7 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 @@ -61,8 +61,8 @@ object DrivetrainConstants { val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 4.0.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 3.25.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 3.5.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 3.5.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3 @@ -84,10 +84,10 @@ object DrivetrainConstants { val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds - val FRONT_LEFT_MODULE_ZERO = 3.28.radians + PI.radians // good - val FRONT_RIGHT_MODULE_ZERO = 2.9.radians + PI.radians // good - val BACK_LEFT_MODULE_ZERO = 5.65.radians + PI.radians // good - val BACK_RIGHT_MODULE_ZERO = 3.48.radians + PI.radians // good + val FRONT_LEFT_MODULE_ZERO = 3.285.radians + PI.radians // good + val FRONT_RIGHT_MODULE_ZERO = 2.91.radians + PI.radians // good + val BACK_LEFT_MODULE_ZERO = 5.665.radians + PI.radians // good + val BACK_RIGHT_MODULE_ZERO = 3.50.radians + PI.radians // good val STEERING_COMPENSATION_VOLTAGE = 10.volts val DRIVE_COMPENSATION_VOLTAGE = 12.volts @@ -128,26 +128,26 @@ object DrivetrainConstants { } } - val LIMELIGHT_THETA_KP = 0.0.degrees.perSecond / 1.degrees + val LIMELIGHT_THETA_KP = 4.0.degrees.perSecond / 1.degrees val LIMELIGHT_THETA_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val LIMELIGHT_THETA_KD = - (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val AUTO_THETA_ALLOWED_ERROR = 3.degrees - val AUTO_THETA_PID_KP = (1.05.radians.perSecond / 1.radians) + val AUTO_THETA_PID_KP = (3.8.radians.perSecond / 1.radians) val AUTO_THETA_PID_KI = (0.0.radians.perSecond / (1.radians * 1.seconds)) val AUTO_THETA_PID_KD = - (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.3.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val TELEOP_ALIGN_PID_KP = 3.6.degrees.perSecond / 1.degrees + val TELEOP_ALIGN_PID_KP = 3.8.degrees.perSecond / 1.degrees val TELEOP_ALIGN_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val TELEOP_ALIGN_PID_KD = - (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.3.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val SIM_AUTO_THETA_PID_KP = 10.degrees.perSecond / 1.degrees + val SIM_AUTO_THETA_PID_KP = 4.0.degrees.perSecond / 1.degrees val SIM_AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val SIM_AUTO_THETA_PID_KD = - (1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val MAX_AUTO_ANGULAR_VEL = 270.0.degrees.perSecond val MAX_AUTO_ANGULAR_ACCEL = 600.0.degrees.perSecond.perSecond @@ -158,14 +158,14 @@ object DrivetrainConstants { val STEERING_KFF = 0.0.volts / 1.0.radians.perSecond // 0.0375 - val DRIVE_KP = 1.45.volts / 1.meters.perSecond + val DRIVE_KP = 1.52.volts / 1.meters.perSecond val DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds) val DRIVE_KD = 0.1.volts / 1.meters.perSecond.perSecond val DRIVE_KFF = 12.0.volts / 4.1675.meters.perSecond val DRIVE_KS = 0.177.volts - val DRIVE_KV = 0.12.volts / 1.0.meters.perSecond + val DRIVE_KV = 0.137.volts / 1.0.meters.perSecond val DRIVE_KA = 0.0.volts / 1.0.meters.perSecond.perSecond // val DRIVE_KS = 0.23677.volts diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 3c51361a..4a8606d2 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -67,7 +67,8 @@ object ElevatorConstants { // week 1 amp amgle // val SHOOT_AMP_POSITION = 17.0.degrees - val SHOOT_AMP_POSITION = 15.0.inches + val SHOOT_AMP_SHOOTER_SIDE_POSITION = 12.inches + val SHOOT_AMP_FEEDER_SIDE_POSITION = 15.0.inches val SOURCE_NOTE_OFFSET = 0.0.inches val ELEVATOR_THETA_POS = 0.0.degrees val HOMING_STATOR_CURRENT = 3.0.amps diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index be4589f1..f75d2e91 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -36,12 +36,12 @@ object FlywheelConstants { val RIGHT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds val RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps - val LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 50.0.amps + val LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 60.0.amps val LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 1.0.amps val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps - val FLYWHEEL_TOLERANCE = 150.0.rotations.perMinute + val FLYWHEEL_TOLERANCE = 75.0.rotations.perMinute object PID { val REAL_KP: ProportionalGain, Volt> = 0.00.volts / 1.0.rotations.perMinute val REAL_KI: IntegralGain, Volt> = @@ -57,7 +57,7 @@ object FlywheelConstants { val REAL_FLYWHEEL_KS = 0.0.volts val REAL_FLYWHEEL_KV = 0.245.volts / 1.radians.perSecond - val REAL_FLYWHEEL_KA = 1.2.volts / 1.radians.perSecond.perSecond + val REAL_FLYWHEEL_KA = 1.8.volts / 1.radians.perSecond.perSecond val SIM_FLYWHEEL_KS = 0.0.volts val SIM_FLYWHEEL_KV = 0.1.volts / 1.radians.perSecond @@ -68,10 +68,12 @@ object FlywheelConstants { val SPEAKER_VELOCITY = 2500.rotations.perMinute val PASSING_SHOT_VELOCITY = 2_000.rotations.perMinute - val AMP_VELOCITY = 1_000.rotations.perMinute + val UNDER_STAGE_SHOT_VELOCITY = 2_500.rotations.perMinute + + val AMP_VELOCITY = 1_500.rotations.perMinute val TRAP_VELOCITY = 3_000.rotations.perMinute - val AMP_SCORE_TIME = 0.1.seconds - val SPEAKER_SCORE_TIME = 0.1.seconds + val AMP_SCORE_TIME = 0.2.seconds + val SPEAKER_SCORE_TIME = 0.2.seconds val EJECT_VELOCITY = 3_000.rotations.perMinute } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 0682b554..65a05646 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -4,6 +4,7 @@ import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.volts @@ -12,18 +13,22 @@ object IntakeConstants { val CENTER_WHEEL_INERTIA = 0.002459315 val VOLTAGE_COMPENSATION = 12.0.volts - val SIM_INTAKE_DISTANCE = 8.inches + val SIM_INTAKE_DISTANCE = 16.inches val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees) // TODO: Change gear ratio according to robot - val ROLLER_CURRENT_LIMIT = 80.0.amps + val ROLLER_CURRENT_LIMIT = 80.amps + val ROLLER_SUPPLY_CURRENT_LIMIT = 120.0.amps + val ROLLER_STATOR_CURRENT_LIMIT = 200.0.amps + val ROLLER_CURRENT_TIME_THRESHOLD = 1.5.seconds + val ROLLER_SUPPLY_TRIGGER_THRESHOLD = 65.amps const val ROLLER_MOTOR_INVERTED = true const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated const val CENTER_WHEEL_GEAR_RATIO = 34.0 / 14.0 // TODO: Update the idle roller voltage later - val IDLE_ROLLER_VOLTAGE = 0.0.volts + val IDLE_ROLLER_VOLTAGE = 2.0.volts val IDLE_CENTER_WHEEL_VOLTAGE = 0.0.volts val INTAKE_ROLLER_VOLTAGE = -12.volts diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt index 4d135228..c17767dd 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -7,23 +7,33 @@ import org.team4099.lib.units.derived.volts object LEDConstants { val INTAKE_CURRENT_THRESHOLD = 15.amps val OUTAKE_CURRENT_THRESHOLD = 20.amps + val BATTERY_FULL_THRESHOLD = 12.5.volts val LED_COUNT = 50 - val BATTERY_WARNING_THRESHOLD = 12.3.volts - enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int) { // Gold NO_NOTE(null, 0, 0, 0), NOTHING(null, 0, 0, 0), + GOLD(null, 255, 105, 0), RED(null, 255, 0, 0), + LIGHT_RED(null, 255, 67, 36), + ORANGE(null, 255, 60, 0), BLUE(null, 0, 0, 255), + PURPLE(null, 67, 36, 255), + GREEN(null, 0, 255, 0), + MAGENTA(null, 255, 0, 255), // Blue HAS_NOTE(null, 0, 0, 255), - - // Red - LOW_BATTERY(null, 255, 105, 0), + NO_TAG(null, 255, 0, 0), + SEES_TAG(null, 255, 105, 0), + SEES_NOTE(null, 255, 60, 0), + + // Yellow + BATTERY_DISPLAY(null, 255, 105, 0), + LOW_BATTERY_WARNING(null, 67, 36, 255), + WHITE(null, 255, 255, 255), // Green diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt index 4083cfa2..9cce5850 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt @@ -13,40 +13,66 @@ object SuperstructureConstants { FieldConstants.subwooferX + FieldConstants.edgeOfBumperToCenter, 3000.rotations.perMinute ), - Pair(55.78.inches, 3000.rotations.perMinute), - Pair(60.0.inches, 3000.rotations.perMinute), - Pair(71.2.inches, 3000.rotations.perMinute), - Pair(83.37.inches, 3500.rotations.perMinute), - Pair(91.2.inches, 3500.rotations.perMinute), - Pair(103.6.inches, 3500.rotations.perMinute), - Pair(114.0.inches, 3500.rotations.perMinute), - Pair(124.0.inches, 3500.rotations.perMinute), - Pair(128.5.inches, 3500.rotations.perMinute), - Pair(134.inches, 3500.rotations.perMinute), - Pair(146.9.inches, 4000.rotations.perMinute), - Pair(159.0.inches, 4000.rotations.perMinute), - Pair(173.9.inches, 4500.rotations.perMinute), - Pair(183.9.inches, 4500.rotations.perMinute), - Pair(194.6.inches, 4500.rotations.perMinute) + Pair(55.55.inches, 2500.rotations.perMinute), + Pair(63.89.inches, 2500.rotations.perMinute), + Pair(75.7.inches, 2500.rotations.perMinute), + Pair(84.25.inches, 2500.rotations.perMinute), + Pair(92.6.inches, 2500.rotations.perMinute), + Pair(103.7.inches, 2500.rotations.perMinute), + Pair(113.2.inches, 2500.rotations.perMinute), + Pair(122.1.inches, 3000.rotations.perMinute), + Pair(134.inches, 3000.rotations.perMinute), + Pair(144.8.inches, 3000.rotations.perMinute), + Pair(156.8.inches, 3500.rotations.perMinute), + Pair(172.2.inches, 3500.rotations.perMinute), + Pair(185.6.inches, 4000.rotations.perMinute), + Pair(198.15.inches, 4000.rotations.perMinute), + Pair(220.1.inches, 4400.rotations.perMinute) ) val distanceWristAngleTableReal = listOf( Pair(FieldConstants.subwooferX + FieldConstants.edgeOfBumperToCenter, -34.5.degrees), - Pair(49.7.inches, -30.degrees), - Pair(60.0.inches, -22.degrees), - Pair(71.2.inches, -16.degrees), - Pair(83.37.inches, -9.degrees), - Pair(91.2.inches, -6.degrees), - Pair(103.6.inches, -1.degrees), - Pair(114.0.inches, -2.degrees), - Pair(124.0.inches, 4.degrees), - Pair(134.inches, 6.degrees), - Pair(146.9.inches, 8.75.degrees), - Pair(159.0.inches, 9.5.degrees), - Pair(173.9.inches, 12.25.degrees), - Pair(183.9.inches, 12.5.degrees), - Pair(194.6.inches, 13.5.degrees) + Pair(55.55.inches, -30.degrees), + Pair(63.89.inches, -25.degrees), + Pair(75.5.inches, -20.degrees), + Pair(84.25.inches, -17.degrees), + Pair(92.6.inches, -15.25.degrees), + Pair(103.7.inches, -13.degrees), + Pair(113.23.inches, -11.degrees), + Pair(122.0.inches, -10.5.degrees), + Pair(134.inches, -9.degrees), + Pair(143.8.inches, -6.75.degrees), + Pair(156.8.inches, -5.5.degrees), + Pair(166.9.inches, -4.5.degrees), + Pair(172.2.inches, -4.375.degrees), + Pair(185.6.inches, -2.95.degrees), + Pair(198.15.inches, -2.2.degrees), + Pair(220.1.inches, -2.degrees) + ) + + val highDistanceFlywheelSpeedTableReal = + listOf( + Pair(71.2.inches, 2500.rotations.perMinute), + Pair(82.0.inches, 2500.rotations.perMinute), + Pair(95.23.inches, 2500.rotations.perMinute), + Pair(108.7.inches, 2500.rotations.perMinute), + Pair(119.0.inches, 2500.rotations.perMinute), + Pair(133.5.inches, 3000.rotations.perMinute), + Pair(143.0.inches, 3000.rotations.perMinute), + Pair(156.6.inches, 3500.rotations.perMinute) + ) + + val highDistanceWristAngleTableReal = + listOf( + Pair(71.2.inches, -10.degrees), + Pair(82.0.inches, -7.25.degrees), + Pair(95.23.inches, -5.5.degrees), + Pair(108.7.inches, -5.degrees), + Pair(119.0.inches, -4.25.degrees), + Pair(133.5.inches, -3.65.degrees), + Pair(143.0.inches, -1.5.degrees), + Pair(156.6.inches, 0.degrees), ) val distanceFlywheelSpeedTableSim = diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index 2903f1aa..93715b3d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -77,15 +77,15 @@ object VisionConstants { } object Limelight { - val LIMELIGHT_NAME = "limelight-zapdos" + val LIMELIGHT_NAME = "limelight-owl" val HORIZONTAL_FOV = 59.6.degrees val VERITCAL_FOV = 45.7.degrees val HIGH_TAPE_HEIGHT = 43.875.inches + 1.inches val MID_TAPE_HEIGHT = 23.905.inches + 1.inches val LL_TRANSFORM = Transform3d( - Translation3d(1.1438.inches, 10.3966.inches, 12.9284.inches), - Rotation3d(8.159.degrees, -90.degrees + 61.610.degrees, -14.1254.degrees) + Translation3d(-14.655.inches, 0.inches, 23.316.inches), + Rotation3d(0.degrees, 143.degrees, 180.degrees) ) const val RES_WIDTH = 320 const val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 8a193c77..8f4d6f13 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -20,7 +20,7 @@ object WristConstants { // val ROLLER_GEAR_RATIO = 0.0 // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps - + val UNDER_STAGE_SHOT = 25.5.degrees val PUSH_DOWN_VOLTAGE = -0.5.volts val EJECT_ANGLE = -15.degrees @@ -29,17 +29,18 @@ object WristConstants { val NOTE_ANGLE_SIM_OFFSET = -24.degrees - val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 1.06488 / 1.0 + val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 69.0 / 20.0 val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO = - 5.0 / 1.0 * 4.0 / 1.0 * 3.0 / 1.0 * 46.0 / 42.0 * 90.0 / 33.0 * 1.0 / 1.06488 + 5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 34.0 * 90.0 / 33.0 * 1.0 / (69.0 / 20.0) val VOLTAGE_COMPENSATION = 12.0.volts val ABSOLUTE_ENCODER_OFFSET = ( - 97.72227856659904.degrees - 35.degrees + 1.3.degrees - - 0.33.degrees - + 97.72227856659904.degrees - 35.degrees + 1.90.degrees - + 0.5.degrees - + 1.1.degrees - // add to drop angle 1.degrees - - 0.5.degrees + 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared @@ -58,28 +59,28 @@ object WristConstants { val WRIST_ZERO_SIM_OFFSET = 27.5.degrees val MAX_WRIST_VELOCITY = 300.degrees.perSecond - val MAX_WRIST_ACCELERATION = 1000.degrees.perSecond.perSecond + val MAX_WRIST_ACCELERATION = 1500.degrees.perSecond.perSecond val HARDSTOP_OFFSET = 47.degrees object PID { val ARBITRARY_FEEDFORWARD = 0.03.volts - val REAL_KP: ProportionalGain = 0.45.volts / 1.0.degrees + val REAL_KP: ProportionalGain = 0.72.volts / 1.0.degrees val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val REAL_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond val FIRST_STAGE_POS_SWITCH_THRESHOLD = 3.0.degrees - val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 5.0.degrees.perSecond + val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 3.0.degrees.perSecond - val FIRST_STAGE_KP: ProportionalGain = 0.35.volts / 1.0.degrees + val FIRST_STAGE_KP: ProportionalGain = 0.69.volts / 1.0.degrees val FIRST_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val FIRST_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond val SECOND_STAGE_POS_SWITCH_THRESHOLD = 1.0.degrees val SECOND_STAGE_VEL_SWITCH_THRESHOLD = 5.0.degrees.perSecond - val SECOND_STAGE_KP: ProportionalGain = 1.5.volts / 1.0.degrees + val SECOND_STAGE_KP: ProportionalGain = 3.volts / 1.0.degrees val SECOND_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) val SECOND_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond @@ -98,19 +99,19 @@ object WristConstants { val SIM_WRIST_KS = 0.15.volts } - val WRIST_TOLERANCE = 0.3.degrees + val WRIST_TOLERANCE = 0.25.degrees - val IDLE_ANGLE = (-34).degrees + val IDLE_ANGLE = (-33.5).degrees - val AMP_SCORE_ANGLE = -8.0.degrees - val FAST_AMP_ANGLE = 35.degrees - val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = -34.degrees + val AMP_SCORE_ANGLE = -12.0.degrees + val FAST_AMP_ANGLE = 27.degrees + val SUBWOOFER_SPEAKER_SHOT_ANGLE_LOW = -33.5.degrees val SUBWOOFER_SPEAKER_SHOT_ANGLE_MID = 8.0.degrees val SUBWOOFER_SPEAKER_SHOT_ANGLE_HIGH = -2.degrees - val CLIMB_ANGLE = 10.0.degrees + val CLIMB_ANGLE = 0.0.degrees val TRAP_ANGLE = 35.degrees - val INTAKE_ANGLE = (-34).degrees + val INTAKE_ANGLE = (-33.75).degrees val IDLE_ANGLE_HAS_GAMEPEICE = -34.degrees - val PASSING_SHOT_ANGLE = -20.degrees + val PASSING_SHOT_ANGLE = -34.degrees } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 80d8b905..9812deaa 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -47,6 +47,7 @@ import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inRotation2ds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import java.util.concurrent.locks.Lock @@ -470,6 +471,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) } + Logger.recordOutput( + "Drivetrain/omegaDegreesPerSecond", desiredChassisSpeeds.omega.inDegreesPerSecond + ) + if (DrivetrainConstants.MINIMIZE_SKEW) { val velocityTransform = Transform2d( @@ -657,7 +662,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fun zeroSensors(isInAutonomous: Boolean) { zeroGyroPitch() zeroGyroRoll() - zeroSteering() + zeroSteering(isInAutonomous) if (!isInAutonomous) { zeroDrive() @@ -724,8 +729,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } /** Zeros the steering motors for each swerve module. */ - fun zeroSteering() { - swerveModules.forEach { it.zeroSteering() } + fun zeroSteering(isInAutonomous: Boolean = false) { + swerveModules.forEach { it.zeroSteering(isInAutonomous) } } /** Zeros the drive motors for each swerve module. */ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index d60a078a..a31c4659 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -360,8 +360,8 @@ class SwerveModule(val io: SwerveModuleIO) { } /** Zeros the steering motor */ - fun zeroSteering() { - io.zeroSteering() + fun zeroSteering(isInAutonomous: Boolean = false) { + io.zeroSteering(isInAutonomous) } /** Zeros the drive motor */ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt index b7cfcbb2..508d258b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt @@ -141,7 +141,7 @@ interface SwerveModuleIO { fun runCharacterization(input: ElectricalPotential) {} fun resetModuleZero() {} - fun zeroSteering() {} + fun zeroSteering(isInAutonomous: Boolean = false) {} fun zeroDrive() {} fun setDriveBrakeMode(brake: Boolean) {} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index 19ac3d21..5c4b3f42 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt @@ -250,7 +250,7 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { println("Zero drive do anything meaningful in sim") } - override fun zeroSteering() { + override fun zeroSteering(isInAutonomous: Boolean) { turnAbsolutePosition = 0.0.radians } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 320d5602..e695222e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -151,6 +151,7 @@ class SwerveModuleIOTalon( DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune + driveConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast driveFalcon.configurator.apply(driveConfiguration) @@ -337,7 +338,7 @@ class SwerveModuleIOTalon( println("Absolute Potentiometer Value $label ($potentiometerOutput") } - override fun zeroSteering() { + override fun zeroSteering(isInAutonomous: Boolean) { steeringFalcon.setPosition( steeringSensor.positionToRawUnits( if (label != Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 13b46153..dce1ba8d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -128,12 +128,20 @@ class Elevator(val io: ElevatorIO) : SubsystemBase() { ElevatorConstants.SHOOT_SPEAKER_HIGH_POSITION, Pair({ it.inInches }, { it.inches }) ) - val shootAmpPosition = + val shootAmpFeederPosition = LoggedTunableValue( - "Elevator/shootAmpPosition", - ElevatorConstants.SHOOT_AMP_POSITION, + "Elevator/shootAmpFeederPosition", + ElevatorConstants.SHOOT_AMP_FEEDER_SIDE_POSITION, Pair({ it.inInches }, { it.inches }) ) + + val shootAmpShooterPosition = + LoggedTunableValue( + "Elevator/shootAmpShooterPosition", + ElevatorConstants.SHOOT_AMP_SHOOTER_SIDE_POSITION, + Pair({ it.inInches }, { it.inches }) + ) + val sourceNoteOffset = LoggedTunableValue( "Elevator/sourceNoteOffset", diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index a5e20665..28013a0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -37,7 +37,7 @@ interface ElevatorIO { var leaderTempCelcius = 0.0.celsius var followerTempCelcius = 0.0.celsius - var isSimulating = false + var isSimulating = true override fun toLog(table: LogTable) { table?.put("elevatorPositionInches", elevatorPosition.inInches) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index 1b26b753..d8c4f3f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -5,7 +5,6 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -89,6 +88,13 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { FlywheelConstants.PASSING_SHOT_VELOCITY, Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) ) + + val underStageShotVelocity = + LoggedTunableValue( + "Flywheel/underStageShotVelocity", + FlywheelConstants.UNDER_STAGE_SHOT_VELOCITY, + Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) + ) } private val kP = @@ -205,7 +211,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { io.updateInputs(inputs) Logger.processInputs("Flywheel", inputs) - DebugLogger.recordDebugOutput( + Logger.recordOutput( "Flywheel/targetDifference", (inputs.leftFlywheelVelocity - flywheelLeftTargetVelocity) .absoluteValue @@ -217,15 +223,17 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { Logger.recordOutput("Flywheel/isAtTargetedVelocity", isAtTargetedVelocity) + Logger.recordOutput("Flywheel/FlywheelRightTargetVoltage", flywheelTargetVoltage.inVolts) + Logger.recordOutput("Flywheel/FlywheelLeftTargetVoltage", flywheelTargetVoltage.inVolts) + + Logger.recordOutput( + "Flywheel/FlywheelRightTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute + ) + Logger.recordOutput( + "Flywheel/FlywheelLeftTargetVelocity", flywheelLeftTargetVelocity.inRotationsPerMinute + ) if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("Flywheel/FlywheelRightTargetVoltage", flywheelTargetVoltage.inVolts) - Logger.recordOutput("Flywheel/FlywheelLeftTargetVoltage", flywheelTargetVoltage.inVolts) - Logger.recordOutput( - "Flywheel/FlywheelRightTargetVelocity", flywheelRightTargetVelocity.inRotationsPerMinute - ) - Logger.recordOutput( - "Flywheel/FlywheelLeftTargetVelocity", flywheelLeftTargetVelocity.inRotationsPerMinute - ) + Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastRightFlywheelVoltage.inVolts) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt index 46560dc3..1dd4b91f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt @@ -41,7 +41,7 @@ interface FlywheelIO { var leftFlywheelDutyCycle = 0.0.volts var leftFlywheelTorque = 0.0.newtons - var isSimulated = true + var isSimulated = false override fun toLog(table: LogTable) { table.put("flywheelRightVelocityRPM", rightFlywheelVelocity.inRotationsPerMinute) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt index f3b00ed8..7c9101b9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -25,6 +25,8 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential @@ -39,6 +41,7 @@ import org.team4099.lib.units.derived.newtons import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerMinute +import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond object FlywheelIOTalon : FlywheelIO { @@ -90,8 +93,7 @@ object FlywheelIOTalon : FlywheelIO { flywheelLeftTalon.clearStickyFaults() flywheelRightTalon.clearStickyFaults() - flywheelLeftConfiguration.Slot0.kP = - flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP) + flywheelLeftConfiguration.Slot0.kP = 0.4 flywheelLeftConfiguration.Slot0.kI = flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI) flywheelLeftConfiguration.Slot0.kD = @@ -102,16 +104,16 @@ object FlywheelIOTalon : FlywheelIO { // // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) - // flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = - // FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes - // flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = - // FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes - // flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = - // FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds - // flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - // flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = - // FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes - // flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false flywheelLeftConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive @@ -181,10 +183,11 @@ object FlywheelIOTalon : FlywheelIO { kV: VelocityFeedforward ) { val PIDLeftConfig = Slot0Configs() - PIDLeftConfig.kP = flywheelLeftSensor.proportionalVelocityGainToRawUnits(kP) + PIDLeftConfig.kP = 0.4 PIDLeftConfig.kI = flywheelLeftSensor.integralVelocityGainToRawUnits(kI) PIDLeftConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(kD) PIDLeftConfig.kV = flywheelLeftSensor.velocityFeedforwardToRawUnits(kV) + PIDLeftConfig.kA = FlywheelConstants.PID.REAL_FLYWHEEL_KA.value flywheelLeftTalon.configurator.apply(PIDLeftConfig) @@ -216,18 +219,25 @@ object FlywheelIOTalon : FlywheelIO { // ) // ) - flywheelLeftTalon.setControl( - MotionMagicVelocityVoltage( - flywheelLeftSensor.velocityToRawUnits(velocity), - flywheelLeftSensor.accelerationToRawUnits(5000.rotations.perSecond.perSecond), - false, - 0.0.volts.inVolts, - 0, - true, - false, - false + var acceleration = + flywheelLeftSensor.accelerationToRawUnits(20000.rotations.perSecond.perSecond) + + if (velocity.absoluteValue < 50.rotations.perMinute) { + flywheelLeftTalon.setControl(VoltageOut(0.0.volts.inVolts)) + } else { + flywheelLeftTalon.setControl( + MotionMagicVelocityVoltage( + flywheelLeftSensor.velocityToRawUnits(velocity), + acceleration, + false, + 0.0.volts.inVolts, + 0, + true, + false, + false + ) ) - ) + } } private fun updateSignals() { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index 45701e41..52801981 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -28,7 +28,7 @@ interface IntakeIO { var centerWheelStatorCurrent = 0.0.amps var centerWheelTemp = 0.0.celsius - var isSimulated = false + var isSimulated = true override fun toLog(table: LogTable?) { table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt index 50c171c3..3bf87ce7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt @@ -56,11 +56,15 @@ object IntakeIOFalconNEO : IntakeIO { var rollerFalconConfiguration = TalonFXConfiguration() rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = - IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes - rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = - IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimit = + IntakeConstants.ROLLER_SUPPLY_CURRENT_LIMIT.inAmperes + // rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = + // IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds + // rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = + // IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true - rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = false + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive rollerFalconConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast @@ -76,7 +80,7 @@ object IntakeIOFalconNEO : IntakeIO { "Roller", MotorCollection( mutableListOf(Falcon500(rollerFalcon, "Roller Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT, 60.celsius, 50.amps, 120.celsius @@ -87,7 +91,7 @@ object IntakeIOFalconNEO : IntakeIO { centerWheelSparkMax.clearFaults() centerWheelSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) - centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) + // centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) centerWheelSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kCoast @@ -99,7 +103,7 @@ object IntakeIOFalconNEO : IntakeIO { "Center Wheel", MotorCollection( mutableListOf(Neo(centerWheelSparkMax, "Center Wheel Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT, 60.celsius, 50.amps, 120.celsius diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt index 46cb1fa8..cdabd6de 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt @@ -47,7 +47,7 @@ object IntakeIOSim : IntakeIO { inputs.centerWheelStatorCurrent = centerWheelSim.currentDrawAmps.amps inputs.centerWheelTemp = 0.celsius - inputs.isSimulated = true + inputs.isSimulated = false } override fun setVoltage( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt index 26ac3c09..0ede25f5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt @@ -3,8 +3,11 @@ package com.team4099.robot2023.subsystems.led import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.derived.ElectricalPotential interface LedIO { + var batteryVoltage: ElectricalPotential + class LedIOInputs : LoggableInputs { var ledState = LEDConstants.CandleState.NO_NOTE.toString() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index e14e188b..04453836 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -4,11 +4,19 @@ import com.ctre.phoenix.led.CANdle import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.volts +import kotlin.math.absoluteValue object LedIOCandle : LedIO { private val ledController = CANdle(Constants.LED.LED_CANDLE_ID) private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE + private var waveRuns = 0 + private var loopCycles = 0 + private var reverseLEDS = false + private var finishedFade = false + override var batteryVoltage: ElectricalPotential = 12.0.volts override fun updateInputs(inputs: LedIO.LedIOInputs) { inputs.ledState = lastState.name @@ -20,8 +28,177 @@ object LedIOCandle : LedIO { setCANdleState(newState) } + private fun wave( + state: LEDConstants.CandleState, + defaultState: LEDConstants.CandleState, + lengthOfWave: Int = 10 + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, waveRuns, lengthOfWave) + + // Set outer colors + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns) + ledController.setLEDs( + defaultState.r, + defaultState.g, + defaultState.b, + 0, + waveRuns + lengthOfWave, + LEDConstants.LED_COUNT - waveRuns - lengthOfWave + ) + + if (waveRuns >= LEDConstants.LED_COUNT - lengthOfWave) { + reverseLEDS = true + } else if (waveRuns < lengthOfWave) { + reverseLEDS = false + } + + waveRuns += if (!reverseLEDS) 1 else -1 + } + + private fun gradient( + state: LEDConstants.CandleState, + otherState: LEDConstants.CandleState, + lengthOfSolidEnds: Int = 10 + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds) + ledController.setLEDs( + otherState.r, + otherState.g, + otherState.b, + 0, + LEDConstants.LED_COUNT - lengthOfSolidEnds, + lengthOfSolidEnds + ) + + for ( + (step, ledIdx) in + (lengthOfSolidEnds until (LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex() + ) { + ledController.setLEDs( + ( + state.r + + (otherState.r - state.r) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + ( + state.g + + (otherState.g - state.g) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + ( + state.b + + (otherState.b - state.b) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + 0, + ledIdx, + 1 + ) + } + } + + private fun progressBar( + state: LEDConstants.CandleState, + defaultState: LEDConstants.CandleState, + percent: Double + ) { + ledController.setLEDs( + state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt() + ) + ledController.setLEDs( + defaultState.r, + defaultState.g, + defaultState.b, + 0, + (percent * LEDConstants.LED_COUNT).toInt(), + LEDConstants.LED_COUNT + ) + } + + private fun fadeBetweenColors( + state: LEDConstants.CandleState, + otherState: LEDConstants.CandleState, + loopCyclesToConverge: Int = 10 + ) { + val stepUpInLoopCycles = 2 + var reachedStartColor = true + var reachedEndColor = true + + for ( + (ledIdx, numberOfLoopCycles) in + ( + loopCyclesToConverge..loopCyclesToConverge + + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles + ) + .withIndex() + ) { + val calculatedR = + ( + state.r + + (otherState.r - state.r) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + val calculatedG = + ( + state.g + + (otherState.g - state.g) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + val calculatedB = + ( + state.b + + (otherState.b - state.b) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + + reachedStartColor = + (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b) + reachedEndColor = + ( + (calculatedR - otherState.r).absoluteValue < 5 && + (calculatedG - otherState.g).absoluteValue < 5 && + (calculatedB - otherState.b).absoluteValue < 5 + ) + + ledController.setLEDs(calculatedR, calculatedG, calculatedB, 0, ledIdx, 1) + } + + loopCycles += if (finishedFade) -1 else 1 + + if (reachedStartColor) { + finishedFade = false + } + + if (reachedEndColor) { // All LEDs are at the end color + finishedFade = true + } + } + private fun setCANdleState(state: LEDConstants.CandleState) { - if (state.animation == null) { + if (state == LEDConstants.CandleState.LOW_BATTERY_WARNING) { + wave(state, LEDConstants.CandleState.NOTHING) + } else if (state == LEDConstants.CandleState.BLUE) { + ledController.clearAnimation(0) + fadeBetweenColors(state, LEDConstants.CandleState.MAGENTA, loopCyclesToConverge = 2) + } else if (state == LEDConstants.CandleState.RED) { + ledController.clearAnimation(0) + fadeBetweenColors(LEDConstants.CandleState.ORANGE, state, loopCyclesToConverge = 2) + } else if (state.animation == null) { ledController.clearAnimation(0) ledController.setLEDs(state.r, state.g, state.b) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 7dd0f7b9..4a5d674b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -1,17 +1,22 @@ package com.team4099.robot2023.subsystems.led import com.team4099.robot2023.config.constants.LEDConstants +import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.derived.volts class Leds(val io: LedIO) { var inputs = LedIO.LedIOInputs() var hasNote = false var subsystemsAtPosition = false - var isIdle = true - var batteryIsLow = false + var isAutoAiming = false + var isAmping = false + var seesGamePiece = false + var seesTag = true var state = LEDConstants.CandleState.NO_NOTE set(value) { @@ -21,29 +26,46 @@ class Leds(val io: LedIO) { fun periodic() { io.updateInputs(inputs) - if (batteryIsLow && DriverStation.isDisabled()) { - state = LEDConstants.CandleState.LOW_BATTERY - } else if (DriverStation.isDisabled()) { - if (DriverStation.getAlliance().isPresent) { - if (FMSData.isBlue) { - state = LEDConstants.CandleState.BLUE + if (DriverStation.getAlliance().isEmpty) { + io.batteryVoltage = RobotController.getBatteryVoltage().volts + + state = + if (io.batteryVoltage < 12.3.volts) { + LEDConstants.CandleState.LOW_BATTERY_WARNING } else { - state = LEDConstants.CandleState.RED + LEDConstants.CandleState.GOLD } + } else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) { + if (FMSData.isBlue) { + state = LEDConstants.CandleState.BLUE } else { - state = LEDConstants.CandleState.NOTHING + state = LEDConstants.CandleState.RED } } else if (hasNote) { - if (subsystemsAtPosition && !isIdle) { - state = LEDConstants.CandleState.CAN_SHOOT + if (isAutoAiming) { + if (!seesTag) { + state = LEDConstants.CandleState.NO_TAG + } else if (!subsystemsAtPosition) { + state = LEDConstants.CandleState.SEES_TAG + } else { + state = LEDConstants.CandleState.CAN_SHOOT + } + } else if (isAmping) { + if (subsystemsAtPosition) { + state = LEDConstants.CandleState.CAN_SHOOT + } else { + state = LEDConstants.CandleState.HAS_NOTE + } } else { state = LEDConstants.CandleState.HAS_NOTE } + } else if (seesGamePiece) { + state = LEDConstants.CandleState.SEES_NOTE } else { state = LEDConstants.CandleState.NO_NOTE } Logger.processInputs("LED", inputs) - Logger.recordOutput("LED/state", state.name) + DebugLogger.recordDebugOutput("LED/state", state.name) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 00d19361..6e9ac8c0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -63,6 +63,10 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { init {} override fun periodic() { + + visibleGamePieces.clear() + targetGamePieceTx = null + val startTime = Clock.realTimestamp io.updateInputs(inputs) Logger.processInputs("LimelightVision", inputs) @@ -89,6 +93,11 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { } } + Logger.recordOutput( + "Limelight/poseRelativeToRobot", + ((targetGamePiecePose?.toPose2d() ?: Pose2d()) - poseSupplier.invoke()).transform2d + ) + Logger.recordOutput( "LimelightVision/RawLimelightReadingsTx", inputs.gamePieceTargets.map { it.tx.inDegrees }.toDoubleArray() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt index b897f273..1ca6483c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt @@ -1,13 +1,10 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.SuperstructureConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.vision.Vision -import com.team4099.robot2023.util.AllianceFlipUtil import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap -import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Translation2d @@ -27,18 +24,28 @@ import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute import kotlin.math.absoluteValue -import kotlin.math.hypot class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { val flywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() val wristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap() + val highFlywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = + InterpolatingDoubleTreeMap() + val highWristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = + InterpolatingDoubleTreeMap() + val tunableFlywheelInterpolationTable: List, LoggedTunableValue>>> val tunableWristInterpolationTable: List, LoggedTunableValue>> + val tunableHighFlywheelInterpolationTable: + List, LoggedTunableValue>>> + + val tunableHighWristInterpolationTable: + List, LoggedTunableValue>> + val interpolationTestDistance = LoggedTunableValue("AutoAim/TestDistance", 0.0.meters, Pair({ it.inInches }, { it.inches })) @@ -110,8 +117,43 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { } } + tunableHighFlywheelInterpolationTable = + SuperstructureConstants.highDistanceFlywheelSpeedTableReal.mapIndexed { i, it -> + Pair( + LoggedTunableValue( + "AutoAim/HighFlywheelInterpolation/$i/Distance", + it.first, + Pair({ it.inInches }, { it.inches }) + ), + LoggedTunableValue( + "AutoAim/HighFlywheelInterpolation/$i/SpeedRPM", + it.second, + Pair({ it.inRotationsPerMinute }, { it.rotations.perMinute }) + ) + ) + } + + tunableHighWristInterpolationTable = + SuperstructureConstants.highDistanceWristAngleTableReal.mapIndexed { i, it -> + Pair( + LoggedTunableValue( + "AutoAim/HighWristInterpolation/$i/Distance", + it.first, + Pair({ it.inInches }, { it.inches }) + ), + LoggedTunableValue( + "AutoAim/HighWristInterpolation/$i/AngleDegrees", + it.second, + Pair({ it.inDegrees }, { it.degrees }) + ) + ) + } + updateFlywheelInterpolationTable() updateWristInterpolationTable() + + updateHighFlywheelInterpolationTable() + updateHighWristInterpolationTable() } fun periodic() { @@ -150,6 +192,17 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { return wristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees } + fun calculateHighFlywheelSpeed(): AngularVelocity { + return highFlywheelSpeedRPMInterpolationTable.get(calculateDistanceFromSpeaker().inMeters) + .rotations + .perMinute + } + + fun calculateHighWristAngle(): Angle { + return highWristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters) + .degrees + } + fun updateFlywheelInterpolationTable() { flywheelSpeedRPMInterpolationTable.clear() tunableFlywheelInterpolationTable.forEach { @@ -159,6 +212,15 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { } } + fun updateHighFlywheelInterpolationTable() { + highFlywheelSpeedRPMInterpolationTable.clear() + tunableHighFlywheelInterpolationTable.forEach { + highFlywheelSpeedRPMInterpolationTable.put( + it.first.get().inMeters, it.second.get().inRotationsPerMinute + ) + } + } + fun updateWristInterpolationTable() { wristAngleDegreesInterpolationTable.clear() tunableWristInterpolationTable.forEach { @@ -166,32 +228,29 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) { } } + fun updateHighWristInterpolationTable() { + highWristAngleDegreesInterpolationTable.clear() + tunableHighWristInterpolationTable.forEach { + highWristAngleDegreesInterpolationTable.put( + it.first.get().inMeters, it.second.get().inDegrees + ) + } + } + fun calculateDistanceFromSpeaker(): Length { val distance = - if (DriverStation.isAutonomous()) { - val speakerTransformWithOdometry = - drivetrain.fieldTRobot.relativeTo( - AllianceFlipUtil.apply(FieldConstants.centerSpeakerOpening) - ) - Logger.recordOutput( - "AutoAim/speakerTransformWithOdometry", speakerTransformWithOdometry.pose2d - ) - hypot(speakerTransformWithOdometry.x.inMeters, speakerTransformWithOdometry.y.inMeters) - .meters - } else { - Translation2d( - vision.robotTSpeaker.y - - (drivetrain.robotVelocity.y * vision.robotTSpeaker.norm.absoluteValue / 5) - .value - .meters, - vision.robotTSpeaker.x - - (drivetrain.robotVelocity.x * vision.robotTSpeaker.norm.absoluteValue / 5) - .value - .meters - ) - .magnitude - .meters - } + Translation2d( + vision.robotTSpeaker.y - + (drivetrain.robotVelocity.y * vision.robotTSpeaker.norm.absoluteValue / 7) + .value + .meters, + vision.robotTSpeaker.x - + (drivetrain.robotVelocity.x * vision.robotTSpeaker.norm.absoluteValue / 7) + .value + .meters + ) + .magnitude + .meters Logger.recordOutput("AutoAim/currentDistanceInches", distance.inInches) return distance } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 2c5a2140..649a07ec 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -50,6 +50,8 @@ sealed interface Request { class PassingShot() : SuperstructureRequest + class UnderStageShot() : SuperstructureRequest + class Tuning() : SuperstructureRequest } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 0b60eca0..bc4eb6af 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1,10 +1,8 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.hal.Clock -import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.FlywheelConstants -import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.elevator.Elevator @@ -13,12 +11,12 @@ import com.team4099.robot2023.subsystems.flywheel.Flywheel import com.team4099.robot2023.subsystems.intake.Intake import com.team4099.robot2023.subsystems.led.LedIOCandle import com.team4099.robot2023.subsystems.led.Leds +import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.util.NoteSimulation import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -35,11 +33,11 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond class Superstructure( private val intake: Intake, @@ -48,7 +46,8 @@ class Superstructure( private val wrist: Wrist, private val flywheel: Flywheel, private val drivetrain: Drivetrain, - private val vision: Vision + private val vision: Vision, + private val limelight: LimelightVision ) : SubsystemBase() { var wristPushDownVoltage = Wrist.TunableWristStates @@ -87,6 +86,14 @@ class Superstructure( var notes = mutableListOf() + var holdingNote = false + get() { + return feeder.hasNote || + currentState == SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK || + currentState == SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD || + (RobotBase.isSimulation() && noteHoldingID != -1) + } + fun toDoubleArray(somePose: Pose3d): DoubleArray { return doubleArrayOf( somePose.x.inMeters, @@ -100,51 +107,65 @@ class Superstructure( } init { - notes.add(NoteSimulation(0, Pose3d())) - notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT - - FieldConstants.StagingLocations.spikeTranslations.forEach { - notes.add( - NoteSimulation( - notes.size, - Pose3d( - it?.x ?: 0.0.inches, - it?.y ?: 0.0.inches, - FieldConstants.noteThickness / 2.0, - Rotation3d() + if (!RobotBase.isReal()) { + notes.add(NoteSimulation(0, Pose3d())) + notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT + + FieldConstants.StagingLocations.spikeTranslations.forEach { + notes.add( + NoteSimulation( + notes.size, + stagedPose = + Pose3d( + it?.x ?: 0.0.inches, + it?.y ?: 0.0.inches, + FieldConstants.noteThickness / 2.0, + Rotation3d() + ) ) ) - ) - } + } - FieldConstants.StagingLocations.centerlineTranslations.forEach { - notes.add( - NoteSimulation( - notes.size, - Pose3d( - it?.x ?: 0.0.inches, - it?.y ?: 0.0.inches, - FieldConstants.noteThickness / 2.0, - Rotation3d() + FieldConstants.StagingLocations.centerlineTranslations.forEach { + notes.add( + NoteSimulation( + notes.size, + stagedPose = + Pose3d( + it?.x ?: 0.0.inches, + it?.y ?: 0.0.inches, + FieldConstants.noteThickness / 2.0, + Rotation3d() + ) ) ) - ) - } + } - notes.forEach { it.poseSupplier = { drivetrain.fieldTRobot } } - notes.forEach { it.wristAngleSupplier = { wrist.inputs.wristPosition } } - notes.forEach { it.elevatorHeightSupplier = { elevator.inputs.elevatorPosition } } - notes.forEach { it.flywheelAngularVelocitySupplier = { flywheel.inputs.rightFlywheelVelocity } } - notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT + notes.forEach { it.poseSupplier = { drivetrain.fieldTRobot } } + notes.forEach { it.wristAngleSupplier = { wrist.inputs.wristPosition } } + notes.forEach { it.elevatorHeightSupplier = { elevator.inputs.elevatorPosition } } + notes.forEach { + it.flywheelAngularVelocitySupplier = { flywheel.inputs.rightFlywheelVelocity } + } + notes.forEach { it.currentState = NoteSimulation.NoteStates.STAGED } + + notes[0].currentState = NoteSimulation.NoteStates.IN_ROBOT + notes[7].currentState = NoteSimulation.NoteStates.OFF_FIELD + noteHoldingID = 0 + } } override fun periodic() { leds.hasNote = feeder.hasNote - leds.isIdle = currentState == SuperstructureStates.IDLE + leds.isAutoAiming = currentState == SuperstructureStates.AUTO_AIM + leds.isAmping = + (currentState == SuperstructureStates.WRIST_AMP_PREP) || + (currentState == SuperstructureStates.ELEVATOR_AMP_PREP) leds.subsystemsAtPosition = wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition - leds.batteryIsLow = - RobotController.getBatteryVoltage() < LEDConstants.BATTERY_WARNING_THRESHOLD.inVolts + leds.seesGamePiece = limelight.inputs.gamePieceTargets.size > 0 + leds.seesTag = vision.getShotConfidence() + Logger.recordOutput("Superstructure/visionConfidence", vision.getShotConfidence()) val ledLoopStartTime = Clock.realTimestamp leds.periodic() @@ -262,15 +283,10 @@ class Superstructure( if (elevator.isHomed) { nextState = SuperstructureStates.IDLE } - noteHoldingID = 0 } SuperstructureStates.IDLE -> { - intake.currentRequest = - Request.IntakeRequest.OpenLoop( - Intake.TunableIntakeStates.idleRollerVoltage.get(), - Intake.TunableIntakeStates.idleCenterWheelVoltage.get() - ) + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(Feeder.TunableFeederStates.idleVoltage.get()) @@ -290,6 +306,12 @@ class Superstructure( Request.WristRequest.TargetingPosition( Wrist.TunableWristStates.idleAngleHasGamepiece.get() ) + + intake.currentRequest = + Request.IntakeRequest.OpenLoop( + Intake.TunableIntakeStates.idleRollerVoltage.get(), + Intake.TunableIntakeStates.idleCenterWheelVoltage.get() + ) } else { wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.idleAngle.get()) @@ -305,21 +327,18 @@ class Superstructure( nextState = SuperstructureStates.HOME_PREP } is Request.SuperstructureRequest.GroundIntake -> { - nextState = SuperstructureStates.GROUND_INTAKE_PREP + if (!feeder.hasNote) { + nextState = SuperstructureStates.GROUND_INTAKE_PREP + } } is Request.SuperstructureRequest.EjectGamePiece -> { nextState = SuperstructureStates.EJECT_GAME_PIECE_PREP } is Request.SuperstructureRequest.PrepScoreAmp -> { - val currentRotation = drivetrain.odomTRobot.rotation - if ((currentRotation > 0.0.degrees && currentRotation < 180.degrees)) { - nextState = SuperstructureStates.ELEVATOR_AMP_PREP - } else { - nextState = SuperstructureStates.WRIST_AMP_PREP - } + nextState = SuperstructureStates.ELEVATOR_AMP_PREP } is Request.SuperstructureRequest.ScoreSpeaker -> { - if (feeder.hasNote) { + if (feeder.hasNote || (RobotBase.isSimulation() && noteHoldingID != -1)) { nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP } } @@ -345,7 +364,7 @@ class Superstructure( nextState = SuperstructureStates.TUNING } is Request.SuperstructureRequest.AutoAim -> { - if (feeder.hasNote) { + if (feeder.hasNote || noteHoldingID != -1) { nextState = SuperstructureStates.AUTO_AIM } } @@ -355,11 +374,16 @@ class Superstructure( is Request.SuperstructureRequest.PassingShot -> { nextState = SuperstructureStates.PASSING_SHOT_PREP } + is Request.SuperstructureRequest.UnderStageShot -> { + nextState = SuperstructureStates.UNDER_STAGE_SHOT_PREP + } } } SuperstructureStates.GROUND_INTAKE_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.intakeAngle.get()) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.intakeAngle.get(), 1.0.degrees + ) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.GROUND_INTAKE } @@ -378,10 +402,7 @@ class Superstructure( Intake.TunableIntakeStates.intakeRollerVoltage.get(), Intake.TunableIntakeStates.intakeCenterWheelVoltage.get() ) - - if (DriverStation.isTeleop()) { - flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(-6.volts) - } + flywheel.currentRequest = Request.FlywheelRequest.OpenLoop(-2.volts) if (noteHoldingID == -1) { for (note in notes) { @@ -400,36 +421,59 @@ class Superstructure( ) if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) { - if (DriverStation.isTeleop()) { - cleanupStartTime = Clock.fpgaTime - nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP - } else { - currentRequest = Request.SuperstructureRequest.Idle() - nextState = SuperstructureStates.IDLE - } + nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK } when (currentRequest) { is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE } is Request.SuperstructureRequest.PrepScoreSpeakerLow -> { - nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + if (DriverStation.isAutonomous()) { + nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } } is Request.SuperstructureRequest.ScoreSpeaker -> { - nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + if (DriverStation.isAutonomous()) { + nextState = SuperstructureStates.SCORE_SPEAKER_LOW_PREP + } + } + is Request.SuperstructureRequest.ManualScoreSpeakerPrep -> { + if (DriverStation.isAutonomous()) { + nextState = SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP + } } is Request.SuperstructureRequest.AutoAim -> { - nextState = SuperstructureStates.AUTO_AIM + if (DriverStation.isAutonomous()) { + nextState = SuperstructureStates.AUTO_AIM + } } } } - SuperstructureStates.GROUND_INTAKE_CLEAN_UP -> { - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.volts) + SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK -> { + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.0.volts) + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) + if (!feeder.hasNote) { + nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD + } - if (Clock.fpgaTime - cleanupStartTime > FeederConstants.CLEAN_UP_TIME) { + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } + } + SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD -> { + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(1.0.volts) + intake.currentRequest = Request.IntakeRequest.OpenLoop(0.0.volts, 0.0.volts) + if (feeder.hasNote || RobotBase.isSimulation()) { currentRequest = Request.SuperstructureRequest.Idle() nextState = SuperstructureStates.IDLE } + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } } SuperstructureStates.AUTO_AIM -> { val targetFlywheelSpeed = aimer.calculateFlywheelSpeed() @@ -438,6 +482,38 @@ class Superstructure( Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute) Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees) + if (drivetrain.fieldVelocity.magnitude.absoluteValue < 0.1.meters.perSecond) { + flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) + wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) + } else { + flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) + wrist.currentRequest = + Request.WristRequest.TargetingPosition(targetWristAngle, 2.0.degrees) + } + + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + is Request.SuperstructureRequest.ScoreSpeaker -> { + if (flywheel.isAtTargetedVelocity && wrist.isAtTargetedPosition) { + nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime + } + } + is Request.SuperstructureRequest.PrepScoreSpeakerHigh -> { + nextState = SuperstructureStates.SCORE_SPEAKER_HIGH_PREP + } + } + } + SuperstructureStates.HIGH_AIM -> { + + val targetFlywheelSpeed = aimer.calculateHighFlywheelSpeed() + val targetWristAngle = aimer.calculateHighWristAngle() + + Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute) + Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees) + flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed) wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle) @@ -446,18 +522,22 @@ class Superstructure( nextState = SuperstructureStates.IDLE } is Request.SuperstructureRequest.ScoreSpeaker -> { - nextState = SuperstructureStates.SCORE_SPEAKER - shootStartTime = Clock.fpgaTime + if (flywheel.isAtTargetedVelocity && wrist.isAtTargetedPosition) { + nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime + } } } } SuperstructureStates.ELEVATOR_AMP_PREP -> { elevator.currentRequest = Request.ElevatorRequest.TargetingPosition( - Elevator.TunableElevatorHeights.shootAmpPosition.get() + Elevator.TunableElevatorHeights.shootAmpFeederPosition.get() ) wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.ampScoreAngle.get()) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.ampScoreAngle.get(), 2.0.degrees + ) if (elevator.isAtTargetedPosition && wrist.isAtTargetedPosition && @@ -474,15 +554,28 @@ class Superstructure( } } SuperstructureStates.WRIST_AMP_PREP -> { + elevator.currentRequest = + Request.ElevatorRequest.TargetingPosition( + Elevator.TunableElevatorHeights.shootAmpShooterPosition.get() + ) wrist.currentRequest = Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.fastAmpAngle.get()) flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity( Flywheel.TunableFlywheelStates.ampVelocity.get() ) + + if (elevator.isAtTargetedPosition && + wrist.isAtTargetedPosition && + currentRequest is Request.SuperstructureRequest.ScoreAmp + ) { + nextState = SuperstructureStates.SCORE_SPEAKER + shootStartTime = Clock.fpgaTime + } + when (currentRequest) { - is Request.SuperstructureRequest.ScoreAmp -> { - nextState = SuperstructureStates.SCORE_SPEAKER + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE } } } @@ -538,7 +631,7 @@ class Superstructure( } } SuperstructureStates.SCORE_SPEAKER -> { - if (noteHoldingID != -1) { + if (noteHoldingID != -1 && !RobotBase.isReal()) { notes[noteHoldingID].currentState = NoteSimulation.NoteStates.IN_FLIGHT noteHoldingID = -1 } @@ -617,11 +710,15 @@ class Superstructure( is Request.SuperstructureRequest.Idle -> { nextState = SuperstructureStates.IDLE } + is Request.SuperstructureRequest.AutoAim -> { + nextState = SuperstructureStates.HIGH_AIM + } } } SuperstructureStates.MANUAL_SCORE_SPEAKER_PREP -> { flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(flywheelToShootAt) - wrist.currentRequest = Request.WristRequest.TargetingPosition(wristAngleToShootAt) + wrist.currentRequest = + Request.WristRequest.TargetingPosition(wristAngleToShootAt, 1.5.degrees) if (wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && currentRequest is Request.SuperstructureRequest.ScoreSpeaker @@ -712,7 +809,9 @@ class Superstructure( } SuperstructureStates.EJECT_GAME_PIECE_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.ejectAngle.get()) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.ejectAngle.get(), 2.0.degrees + ) if (wrist.isAtTargetedPosition) { nextState = SuperstructureStates.EJECT_GAME_PIECE @@ -726,7 +825,9 @@ class Superstructure( } SuperstructureStates.PASSING_SHOT_PREP -> { wrist.currentRequest = - Request.WristRequest.TargetingPosition(Wrist.TunableWristStates.passingShotAngle.get()) + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.passingShotAngle.get(), 2.0.degrees + ) flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity( Flywheel.TunableFlywheelStates.passingShotVelocity.get() @@ -746,6 +847,31 @@ class Superstructure( } } } + SuperstructureStates.UNDER_STAGE_SHOT_PREP -> { + wrist.currentRequest = + Request.WristRequest.TargetingPosition( + Wrist.TunableWristStates.underStageShotAngle.get(), 2.0.degrees + ) + flywheel.currentRequest = + Request.FlywheelRequest.TargetingVelocity( + Flywheel.TunableFlywheelStates.underStageShotVelocity.get() + ) + + shootStartTime = Clock.fpgaTime + + if (flywheel.isAtTargetedVelocity && + wrist.isAtTargetedPosition && + currentRequest is Request.SuperstructureRequest.ScoreSpeaker + ) { + nextState = SuperstructureStates.SCORE_SPEAKER + } + + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } + } SuperstructureStates.TUNING -> { if (currentRequest is Request.SuperstructureRequest.Idle) { nextState = SuperstructureStates.IDLE @@ -771,13 +897,23 @@ class Superstructure( fun requestIdleCommand(): Command { val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.Idle() }.until { + run { currentRequest = Request.SuperstructureRequest.Idle() }.until { isAtRequestedState && currentState == SuperstructureStates.IDLE } returnCommand.name = "RequestIdleCommand" return returnCommand } + fun forceIdleIfAutoAimCommand(): Command { + val returnCommand = run { + if (currentState == SuperstructureStates.AUTO_AIM) { + currentRequest = Request.SuperstructureRequest.Idle() + } + } + returnCommand.name = "forceIdleIfAutoAim" + return returnCommand + } + fun ejectGamePieceCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.EjectGamePiece() }.until { @@ -808,6 +944,16 @@ class Superstructure( return returnCommand } + fun underStageCommand(): Command { + val returnCommand = + run { currentRequest = Request.SuperstructureRequest.UnderStageShot() }.until { + currentState == SuperstructureStates.UNDER_STAGE_SHOT_PREP + } + + returnCommand.name = "UnderStageShotCommand" + return returnCommand + } + fun homeCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.Home() }.until { @@ -862,7 +1008,8 @@ class Superstructure( val returnCommand = run { if (currentState == SuperstructureStates.ELEVATOR_AMP_PREP || - currentState == SuperstructureStates.WRIST_AMP_PREP + currentState == SuperstructureStates.WRIST_AMP_PREP || + currentState == SuperstructureStates.SCORE_ELEVATOR_AMP ) { currentRequest = Request.SuperstructureRequest.ScoreAmp() } else if (currentState == SuperstructureStates.SCORE_TRAP_PREP) { @@ -876,6 +1023,13 @@ class Superstructure( return returnCommand } + fun noNoteDeadlineCommand(): Command { + val returnCommand = run {}.until { !feeder.hasNote } + + returnCommand.name = "noNoteDeadlineCommand" + return returnCommand + } + fun prepSpeakerMidCommand(): Command { val returnCommand = runOnce { currentRequest = Request.SuperstructureRequest.PrepScoreSpeakerMid() }.until { @@ -932,7 +1086,7 @@ class Superstructure( fun autoAimCommand(): Command { val returnCommand = - runOnce { currentRequest = Request.SuperstructureRequest.AutoAim() }.until { + run { currentRequest = Request.SuperstructureRequest.AutoAim() }.until { isAtRequestedState && currentState == SuperstructureStates.AUTO_AIM } returnCommand.name = "AutoAim" @@ -1024,7 +1178,8 @@ class Superstructure( HOME, GROUND_INTAKE_PREP, GROUND_INTAKE, - GROUND_INTAKE_CLEAN_UP, + GROUND_INTAKE_CLEAN_UP_PUSH_BACK, + GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD, ELEVATOR_AMP_PREP, WRIST_AMP_PREP, SCORE_ELEVATOR_AMP, @@ -1040,8 +1195,9 @@ class Superstructure( EJECT_GAME_PIECE, EJECT_GAME_PIECE_PREP, PASSING_SHOT_PREP, - PASSING_SHOT, - AUTO_AIM + UNDER_STAGE_SHOT_PREP, + AUTO_AIM, + HIGH_AIM } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index b98b9ea8..28f48e95 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -55,7 +55,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { private var visionConsumer: Consumer> = Consumer {} private var speakerVisionConsumer: Consumer = Consumer {} private val lastFrameTimes = mutableMapOf() - private val lastTagDetectionTimes = mutableMapOf() + private var lastDetectionTime = 0.0.seconds init { for (i in io.indices) { @@ -73,6 +73,11 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { this.speakerVisionConsumer = speakerVisionMeasurementConsumer } + fun getShotConfidence(): Boolean { + return Clock.realTimestamp - lastDetectionTime < 4.seconds && + trustedRobotDistanceToTarget < 210.inches + } + override fun periodic() { val startTime = Clock.realTimestamp @@ -97,8 +102,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { var robotDistanceToTarget: Length? = null var tagTargets = inputs[instance].cameraTargets - println(tagTargets.size) - val cornerData = mutableListOf() for (tag in tagTargets) { @@ -109,12 +112,14 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { robotDistanceToTarget = PhotonUtils.calculateDistanceToTargetMeters( - cameraPoses[instance].translation.z.inMeters, - 57.125.inches.inMeters, - 23.25.degrees.inRadians, - tag.pitch.degrees.inRadians - ) - .meters + cameraPoses[instance].translation.z.inMeters, + 57.125.inches.inMeters, + 22.42.degrees.inRadians, + tag.pitch.degrees.inRadians + ) + .meters + 4.inches + + lastDetectionTime = Clock.realTimestamp Logger.recordOutput( "Vision/${VisionConstants.CAMERA_NAMES[instance]}/robotDistanceToTarget", @@ -128,8 +133,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { ) ) - robotTSpeaker = - Translation3d(cameraTspeaker2d.x + 4.inches, cameraTspeaker2d.y, 0.meters) + robotTSpeaker = Translation3d(cameraTspeaker2d.x, cameraTspeaker2d.y, 0.meters) val timestampedTrigVisionUpdate = TimestampedTrigVisionUpdate( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 0d7813b4..69d11d8a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -100,6 +100,13 @@ class Wrist(val io: WristIO) : SubsystemBase() { WristConstants.PASSING_SHOT_ANGLE, Pair({ it.inDegrees }, { it.degrees }) ) + + val underStageShotAngle = + LoggedTunableValue( + "Wrist/underStageShotAngle", + WristConstants.UNDER_STAGE_SHOT, + Pair({ it.inDegrees }, { it.degrees }) + ) val subwooferSpeakerShotAngleLow = LoggedTunableValue( "Wrist/subwooferSpeakerShotAngleLow",