From ddee58a613c29c8c651ba401d83c420e39a4eddc Mon Sep 17 00:00:00 2001 From: spybh66 Date: Mon, 10 Feb 2025 13:08:23 -0500 Subject: [PATCH 01/24] fixed pid issue when on red, but aiming at blue reef --- src/main/java/frc/robot/commands/DriveCommands.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 7970295..72ff42c 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -37,6 +37,7 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -227,6 +228,7 @@ public static Command joystickApproach( .plus(offsetVector); SmartDashboard.putData(alignController); // TODO: Calibrate PID + Logger.recordOutput("AlignDebug/approachTarget", approachTranslation); // Calculate angular speed double omega = @@ -240,15 +242,10 @@ public static Command joystickApproach( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega); - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + drive.getRotation())); }, drive) From 7b969e504ffbd0c6ce3dd03650326cc7632a35ee Mon Sep 17 00:00:00 2001 From: spybh66 Date: Mon, 10 Feb 2025 13:30:32 -0500 Subject: [PATCH 02/24] changing driveatangle to be blue origin --- src/main/java/frc/robot/commands/DriveCommands.java | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 72ff42c..b31fc6c 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -149,15 +149,10 @@ public static Command joystickDriveAtAngle( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega); - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + drive.getRotation())); }, drive) From d152e683f2af25733a68b556b6955b6f5ed6bd51 Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Mon, 10 Feb 2025 21:29:30 -0500 Subject: [PATCH 03/24] tuning work --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Ports.java | 2 +- src/main/java/frc/robot/Robot.java | 39 +----------- src/main/java/frc/robot/RobotContainer.java | 61 +++++++++++++------ .../frc/robot/generated/TunerConstants.java | 28 ++++----- .../Elevator/ElevatorConstants.java | 23 ++++--- 6 files changed, 71 insertions(+), 84 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80b1895..1ed8212 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,7 +11,7 @@ public class Constants { public static double loopPeriodSecs = 0.02; // Use LoggedTunableNumbers - public static final boolean tuningMode = true; + public static final boolean tuningMode = false; /** * This enum defines the runtime mode used by AdvantageKit. The mode is always "real" when diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index e8d9ff0..cae6cdd 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -23,6 +23,6 @@ public class Ports { public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(22, "rio"); // Top Kraken public static final CanDeviceId ELEVATOR_FOLLOWER = new CanDeviceId(23, "rio"); // Bottom Kraken - public static final CanDeviceId CLIMBER = new CanDeviceId(24, "rio"); + public static final CanDeviceId CLIMBER = new CanDeviceId(24, "Drivetrain"); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9ab2420..40827ce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -149,46 +149,9 @@ public void disabledPeriodic() { // Test var alliance = DriverStation.getAlliance(); - Pose2d test; // Test // Get currently selected command - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // Check if is the same as the last one - if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { - // Check if its contained in the list of our autos - if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { - // Clear the current path - m_pathsToShow.clear(); - // Grabs all paths from the auto - try { - for (PathPlannerPath path : PathPlannerAuto - .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { - // Adds all poses to master list - m_pathsToShow.addAll(path.getPathPoses()); - } - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } catch (ParseException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - // Displays all poses on Field2d widget depending on Alliance Side - if (alliance.isPresent() && alliance.get() == Alliance.Red) { - m_autoTraj.getObject("traj").setPoses(m_pathsToShow); - } else { - for (int i = 0; i < m_pathsToShow.size(); i++) { - test = m_pathsToShow.get(0).transformBy( - new Transform2d()); - Logger.recordOutput("m_pathsToShow" + i, m_pathsToShow.get(i)); - Logger.recordOutput("NEW_m_pathsToShow" + i, test); - } - } - // - m_autoTraj.setRobotPose(m_pathsToShow.get(0)); - } - } - m_lastAutonomousCommand = m_autonomousCommand; + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62c9d08..c6f9090 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,7 +75,7 @@ public class RobotContainer { private final Superstructure m_superStruct; public final Vision m_vision; - public final LED m_LED; + // public final LED m_LED; // Trigger for algae/coral mode switching private boolean coralModeEnabled = false; @@ -92,28 +92,49 @@ public RobotContainer() switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations + // m_drive = + // new Drive( + // new GyroIOPigeon2(), + // new ModuleIOTalonFXReal(TunerConstants.FrontLeft), + // new ModuleIOTalonFXReal(TunerConstants.FrontRight), + // new ModuleIOTalonFXReal(TunerConstants.BackLeft), + // new ModuleIOTalonFXReal(TunerConstants.BackRight), + // (robotPose) -> { + // }); + + // m_profiledArm = new Arm(new ArmIOTalonFX(), false); + m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); + // m_profiledClimber = new Climber(new ClimberIOTalonFX(), false); + // m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); + m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIOReal()); + m_intakeLaserCAN = new IntakeLaserCAN(new IntakeLaserCANIOReal()); + + // m_vision = + // new Vision( + // m_drive, + // new VisionIOPhotonVision(camera0Name, robotToCamera0), + // new VisionIOPhotonVision(camera1Name, robotToCamera1)); + + // break; + m_drive = new Drive( - new GyroIOPigeon2(), - new ModuleIOTalonFXReal(TunerConstants.FrontLeft), - new ModuleIOTalonFXReal(TunerConstants.FrontRight), - new ModuleIOTalonFXReal(TunerConstants.BackLeft), - new ModuleIOTalonFXReal(TunerConstants.BackRight), + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, (robotPose) -> { }); - m_profiledArm = new Arm(new ArmIOTalonFX(), false); - m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); - m_profiledClimber = new Climber(new ClimberIOTalonFX(), false); - m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); - m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIOReal()); - m_intakeLaserCAN = new IntakeLaserCAN(new IntakeLaserCANIOReal()); + m_profiledArm = new Arm(new ArmIO() {}, true); + // m_profiledElevator = new Elevator(new ElevatorIO() {}, true); + m_profiledClimber = new Climber(new ClimberIO() {}, true); + m_clawRoller = new ClawRoller(new ClawRollerIO() {}, true); + // m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIO() {}); + // m_intakeLaserCAN = new IntakeLaserCAN(new IntakeLaserCANIO() {}); - m_vision = - new Vision( - m_drive, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1)); + m_vision = new Vision(m_drive, new VisionIO() {}, new VisionIO() {}); break; @@ -182,8 +203,8 @@ public RobotContainer() m_superStruct = new Superstructure(m_profiledArm, m_profiledElevator); // LED subsystem reads status from all other subsystems to control LEDs via CANdle - m_LED = new LED(m_driver, m_profiledArm, m_clawRoller, m_profiledClimber, m_drive, - m_profiledElevator, m_vision, m_clawRollerLaserCAN, isCoralMode); + // m_LED = new LED(m_driver, m_profiledArm, m_clawRoller, m_profiledClimber, m_drive, + // m_profiledElevator, m_vision, m_clawRollerLaserCAN, isCoralMode); // Logic Triggers registerNamedCommands(); @@ -212,7 +233,7 @@ public RobotContainer() m_autoChooser.addOption("Arm static", m_profiledArm.staticCharacterization(2.0)); // Configure the controller button and joystick bindings - configureControllerBindings(); + // configureControllerBindings(); // Detect if controllers are missing / Stop multiple warnings DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 807f5f6..a2b8366 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -80,7 +80,7 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("Drivetrain", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot @@ -97,7 +97,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 13; + private static final int kPigeonId = 8; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -136,9 +136,9 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 9; + private static final int kFrontLeftDriveMotorId = 11; + private static final int kFrontLeftSteerMotorId = 9; + private static final int kFrontLeftEncoderId = 10; private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.393798828125); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = true; @@ -147,9 +147,9 @@ public class TunerConstants { private static final Distance kFrontLeftYPos = Inches.of(9.5); // Front Right - private static final int kFrontRightDriveMotorId = 5; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 10; + private static final int kFrontRightDriveMotorId = 7; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 6; private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.267822265625); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = true; @@ -158,9 +158,9 @@ public class TunerConstants { private static final Distance kFrontRightYPos = Inches.of(-9.5); // Back Left - private static final int kBackLeftDriveMotorId = 3; - private static final int kBackLeftSteerMotorId = 4; - private static final int kBackLeftEncoderId = 11; + private static final int kBackLeftDriveMotorId = 14; + private static final int kBackLeftSteerMotorId = 12; + private static final int kBackLeftEncoderId = 13; private static final Angle kBackLeftEncoderOffset = Rotations.of(0.106689453125); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = true; @@ -169,9 +169,9 @@ public class TunerConstants { private static final Distance kBackLeftYPos = Inches.of(9.5); // Back Right - private static final int kBackRightDriveMotorId = 7; - private static final int kBackRightSteerMotorId = 8; - private static final int kBackRightEncoderId = 12; + private static final int kBackRightDriveMotorId = 4; + private static final int kBackRightSteerMotorId = 2; + private static final int kBackRightEncoderId = 3; private static final Angle kBackRightEncoderOffset = Rotations.of(0.0048828125); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = true; diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java index 11fbe77..424aa7e 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java @@ -31,13 +31,13 @@ public final class ElevatorConstants { kSubSysConstants.kLeaderMotor = Ports.ELEVATOR_MAIN; kSubSysConstants.kFollowMotor = Ports.ELEVATOR_FOLLOWER; - kSubSysConstants.kFollowerOpposesMain = true; + kSubSysConstants.kFollowerOpposesMain = false; // Using TalonFX internal encoder kSubSysConstants.kCANcoder = null; kSubSysConstants.kMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 1.0; + kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 9.6; kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = 1.0; // Using a remote CANcoder @@ -56,7 +56,7 @@ public final class ElevatorConstants { kSubSysConstants.kMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; kSubSysConstants.kMotorConfig.MotorOutput.Inverted = - InvertedValue.CounterClockwise_Positive; + InvertedValue.Clockwise_Positive; kSubSysConstants.kMotorConfig.Voltage.PeakForwardVoltage = 12.0; kSubSysConstants.kMotorConfig.Voltage.PeakReverseVoltage = -12.0; @@ -65,17 +65,20 @@ public final class ElevatorConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimit = 70; kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 5.4; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 0; + kSubSysConstants.kMotorConfig.Slot0.kP = 12; kSubSysConstants.kMotorConfig.Slot0.kI = 0; kSubSysConstants.kMotorConfig.Slot0.kD = 0; kSubSysConstants.kMotorConfig.Slot0.GravityType = GravityTypeValue.Elevator_Static; - kSubSysConstants.kMotorConfig.Slot0.kG = 0; - kSubSysConstants.kMotorConfig.Slot0.kS = 0; - kSubSysConstants.kMotorConfig.Slot0.kV = 0; - kSubSysConstants.kMotorConfig.Slot0.kA = 0; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 0; + kSubSysConstants.kMotorConfig.Slot0.kG = .33; + kSubSysConstants.kMotorConfig.Slot0.kS = .2; + kSubSysConstants.kMotorConfig.Slot0.kV = 1.1; + kSubSysConstants.kMotorConfig.Slot0.kA = .2; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 20; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 5; kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicJerk = 0; /* SIM system profile constants */ From 679b7acda2473e913c275a2ea52904afad22713f Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Tue, 11 Feb 2025 18:48:52 -0500 Subject: [PATCH 04/24] Elevator PID --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 1 + .../subsystems/Elevator/ElevatorConstants.java | 16 ++++++++-------- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ed8212..80b1895 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,7 +11,7 @@ public class Constants { public static double loopPeriodSecs = 0.02; // Use LoggedTunableNumbers - public static final boolean tuningMode = false; + public static final boolean tuningMode = true; /** * This enum defines the runtime mode used by AdvantageKit. The mode is always "real" when diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 40827ce..b12eb3a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -148,6 +148,7 @@ public void disabledInit() public void disabledPeriodic() { // Test + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); var alliance = DriverStation.getAlliance(); // Test // Get currently selected command diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java index 424aa7e..7fdd71d 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/Elevator/ElevatorConstants.java @@ -69,16 +69,16 @@ public final class ElevatorConstants { kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 12; + kSubSysConstants.kMotorConfig.Slot0.kP = 1200; kSubSysConstants.kMotorConfig.Slot0.kI = 0; - kSubSysConstants.kMotorConfig.Slot0.kD = 0; + kSubSysConstants.kMotorConfig.Slot0.kD = 30; kSubSysConstants.kMotorConfig.Slot0.GravityType = GravityTypeValue.Elevator_Static; - kSubSysConstants.kMotorConfig.Slot0.kG = .33; - kSubSysConstants.kMotorConfig.Slot0.kS = .2; - kSubSysConstants.kMotorConfig.Slot0.kV = 1.1; - kSubSysConstants.kMotorConfig.Slot0.kA = .2; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 20; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 5; + kSubSysConstants.kMotorConfig.Slot0.kG = 5; + kSubSysConstants.kMotorConfig.Slot0.kS = 0; + kSubSysConstants.kMotorConfig.Slot0.kV = 0; + kSubSysConstants.kMotorConfig.Slot0.kA = 0; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 75; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 20; kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicJerk = 0; /* SIM system profile constants */ From a437d0583bc2fbb867e3efcab7bba6446791ee3f Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Tue, 11 Feb 2025 21:05:01 -0500 Subject: [PATCH 05/24] 2/11 changes --- src/main/java/frc/robot/RobotContainer.java | 8 ++-- .../robot/subsystems/Arm/ArmConstants.java | 44 ++++++++++--------- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c6f9090..fc65130 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,8 +102,8 @@ public RobotContainer() // (robotPose) -> { // }); - // m_profiledArm = new Arm(new ArmIOTalonFX(), false); - m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); + m_profiledArm = new Arm(new ArmIOTalonFX(), false); + // m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); // m_profiledClimber = new Climber(new ClimberIOTalonFX(), false); // m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIOReal()); @@ -127,8 +127,8 @@ public RobotContainer() (robotPose) -> { }); - m_profiledArm = new Arm(new ArmIO() {}, true); - // m_profiledElevator = new Elevator(new ElevatorIO() {}, true); + // m_profiledArm = new Arm(new ArmIO() {}, true); + m_profiledElevator = new Elevator(new ElevatorIO() {}, true); m_profiledClimber = new Climber(new ClimberIO() {}, true); m_clawRoller = new ClawRoller(new ClawRollerIO() {}, true); // m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIO() {}); diff --git a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java index 8cca6e8..3c959bd 100644 --- a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import frc.robot.Ports; @@ -33,30 +34,30 @@ public final class ArmConstants { // Using TalonFX internal encoder - kSubSysConstants.kCANcoder = null; - kSubSysConstants.kMotorConfig.Feedback.FeedbackSensorSource = - FeedbackSensorSourceValue.RotorSensor; - kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 54.4; - kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = 1.0; + // kSubSysConstants.kCANcoder = null; + // kSubSysConstants.kMotorConfig.Feedback.FeedbackSensorSource = + // FeedbackSensorSourceValue.RotorSensor; + // kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 54.4; + // kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = 1.0; // Using a remote CANcoder - /* - * kSubSysConstants.kCANcoder = Ports.ARM_CANCODER; - * kSubSysConstants.kMotorConfig.Feedback.FeedbackSensorSource = - * FeedbackSensorSourceValue.FusedCANcoder; - * kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 7.04; - * kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = 54.4/7.04; - * kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.3467; - * kSubSysConstants.kEncoderConfig.MagnetSensor.SensorDirection = - * SensorDirectionValue.Clockwise_Positive; - * kSubSysConstants.kEncoderConfig.MagnetSensor.AbsoluteSensorRange = - * AbsoluteSensorRangeValue.Unsigned_0To1; Sprocket ratios = 2.0; Gear Ratios = 3.0; - * Planetary Ratio = 25.0; - */ + + kSubSysConstants.kCANcoder = Ports.ARM_CANCODER; + kSubSysConstants.kMotorConfig.Feedback.FeedbackRemoteSensorID = + Ports.ARM_CANCODER.getDeviceNumber(); + kSubSysConstants.kMotorConfig.Feedback.FeedbackSensorSource = + FeedbackSensorSourceValue.RemoteCANcoder; + kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 1; + kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = (9 / 1) * (48 / 22) * (70 / 22); + kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.826416015625; + kSubSysConstants.kEncoderConfig.MagnetSensor.SensorDirection = + SensorDirectionValue.Clockwise_Positive; + kSubSysConstants.kEncoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; + kSubSysConstants.kMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; kSubSysConstants.kMotorConfig.MotorOutput.Inverted = - InvertedValue.CounterClockwise_Positive; + InvertedValue.Clockwise_Positive; kSubSysConstants.kMotorConfig.Voltage.PeakForwardVoltage = 12.0; kSubSysConstants.kMotorConfig.Voltage.PeakReverseVoltage = -12.0; @@ -65,13 +66,16 @@ public final class ArmConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimit = 70; kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = -0.04; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + /* REAL system profile constants */ kSubSysConstants.kMotorConfig.Slot0.kP = 0; kSubSysConstants.kMotorConfig.Slot0.kI = 0; kSubSysConstants.kMotorConfig.Slot0.kD = 0; kSubSysConstants.kMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; kSubSysConstants.kMotorConfig.Slot0.kG = 0; - kSubSysConstants.kMotorConfig.Slot0.kS = 0; + kSubSysConstants.kMotorConfig.Slot0.kS = 35; kSubSysConstants.kMotorConfig.Slot0.kV = 0; kSubSysConstants.kMotorConfig.Slot0.kA = 0; kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0; From a62945a37e3e19403081bde7b4bc8a511a18e3b0 Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Wed, 12 Feb 2025 21:00:17 -0500 Subject: [PATCH 06/24] Arm PID --- src/main/java/frc/robot/RobotContainer.java | 199 +++++++++--------- .../java/frc/robot/subsystems/Arm/Arm.java | 6 +- .../robot/subsystems/Arm/ArmConstants.java | 20 +- 3 files changed, 114 insertions(+), 111 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc65130..9f3d0ee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ public class RobotContainer { // public final LED m_LED; // Trigger for algae/coral mode switching - private boolean coralModeEnabled = false; + private boolean coralModeEnabled = true; private Trigger isCoralMode = new Trigger(() -> coralModeEnabled); // private final LaserCANSensor m_clawLaserCAN = @@ -103,7 +103,7 @@ public RobotContainer() // }); m_profiledArm = new Arm(new ArmIOTalonFX(), false); - // m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); + m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); // m_profiledClimber = new Climber(new ClimberIOTalonFX(), false); // m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIOReal()); @@ -128,7 +128,7 @@ public RobotContainer() }); // m_profiledArm = new Arm(new ArmIO() {}, true); - m_profiledElevator = new Elevator(new ElevatorIO() {}, true); + // m_profiledElevator = new Elevator(new ElevatorIO() {}, true); m_profiledClimber = new Climber(new ClimberIO() {}, true); m_clawRoller = new ClawRoller(new ClawRollerIO() {}, true); // m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIO() {}); @@ -233,7 +233,7 @@ public RobotContainer() m_autoChooser.addOption("Arm static", m_profiledArm.staticCharacterization(2.0)); // Configure the controller button and joystick bindings - // configureControllerBindings(); + configureControllerBindings(); // Detect if controllers are missing / Stop multiple warnings DriverStation.silenceJoystickConnectionWarning(true); @@ -277,7 +277,7 @@ public Command setCoralAlgaeModeCommand() private void configureControllerBindings() { // Default command, normal field-relative drive - m_drive.setDefaultCommand(joystickDrive()); + // m_drive.setDefaultCommand(joystickDrive()); // Driver Back Button: Reset gyro / odometry final Runnable setPose = @@ -291,41 +291,42 @@ private void configureControllerBindings() Commands.runOnce(setPose).ignoringDisable(true)); // Driver Left Bumper: Face Nearest Reef Face - m_driver.leftBumper() - .whileTrue( - joystickDriveAtAngle( - () -> FieldConstants.getNearestReefFace(m_drive.getPose()).getRotation() - .rotateBy(Rotation2d.k180deg))); + // m_driver.leftBumper() + // .whileTrue( + // joystickDriveAtAngle( + // () -> FieldConstants.getNearestReefFace(m_drive.getPose()).getRotation() + // .rotateBy(Rotation2d.k180deg))); // Driver Left Bumper + Right Stick Right: Approach Nearest Right-Side Reef Branch - m_driver.leftBumper().and(m_driver.axisGreaterThan(XboxController.Axis.kRightX.value, 0.8)) - .whileTrue( - joystickApproach( - () -> FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT))); + // m_driver.leftBumper().and(m_driver.axisGreaterThan(XboxController.Axis.kRightX.value, + // 0.8)) + // .whileTrue( + // joystickApproach( + // () -> FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT))); // Driver Left Bumper + Right Stick Left: Approach Nearest Left-Side Reef Branch - m_driver.leftBumper().and(m_driver.axisLessThan(XboxController.Axis.kRightX.value, -0.8)) - .whileTrue( - joystickApproach( - () -> FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.LEFT))); + // m_driver.leftBumper().and(m_driver.axisLessThan(XboxController.Axis.kRightX.value, -0.8)) + // .whileTrue( + // joystickApproach( + // () -> FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.LEFT))); // Driver Left Bumper + Right Bumper: Approach Nearest Reef Face - m_driver.leftBumper().and(m_driver.rightBumper()) - .whileTrue( - joystickApproach(() -> FieldConstants.getNearestReefFace(m_drive.getPose()))); + // m_driver.leftBumper().and(m_driver.rightBumper()) + // .whileTrue( + // joystickApproach(() -> FieldConstants.getNearestReefFace(m_drive.getPose()))); // Driver A Button: Send Arm and Elevator to LEVEL_1 m_driver - .a().and(isCoralMode) + .a() .onTrue( m_superStruct.getTransitionCommand(Arm.State.LEVEL_1, Elevator.State.LEVEL_1)); // Driver A Button held and Right Bumper Pressed: Send Arm and Elevator to Processor - m_driver - .a().and(isCoralMode.negate()) - .onTrue( - m_superStruct.getTransitionCommand(Arm.State.ALGAE_GROUND, - Elevator.State.ALGAE_SCORE)); + // m_driver + // .a().and(isCoralMode.negate()) + // .onTrue( + // m_superStruct.getTransitionCommand(Arm.State.ALGAE_GROUND, + // Elevator.State.ALGAE_SCORE)); // Driver X Button: Send Arm and Elevator to LEVEL_2 m_driver @@ -341,16 +342,16 @@ private void configureControllerBindings() // Driver Y Button: Send Arm and Elevator to LEVEL_4 m_driver - .y().and(isCoralMode) + .y() .onTrue( m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4)); // Driver Y Button held and Right Bumper having been pressed to ALGAE mode: Send Arm and // Elevator to NET - m_driver - .y().and(isCoralMode.negate()) - .onTrue( - m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.BARGE)); + // m_driver + // .y().and(isCoralMode.negate()) + // .onTrue( + // m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.BARGE)); // TODO: Test Arm Level 4 in Sim // Driver Right Trigger: Place Coral or Algae (Should be done once the robot is in position) @@ -359,81 +360,81 @@ private void configureControllerBindings() // Driver Left Trigger: Drivetrain drive at coral station angle, prepare the elevator and // arm, Get Ready to Intake Coral - m_driver - .leftTrigger().and(isCoralMode) - .whileTrue( - Commands.sequence( - joystickDriveAtAngle( - () -> FieldConstants.getNearestCoralStation(m_drive.getPose()) - .getRotation()), - m_profiledElevator.setStateCommand(Elevator.State.CORAL_INTAKE), - Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) - .andThen(Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.CORAL_INTAKE), - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE)), - Commands.waitUntil(m_clawRollerLaserCAN.triggered) - .andThen(m_clawRoller.setStateCommand(ClawRoller.State.HOLDCORAL)))) - .andThen( - Commands.parallel( - m_clawRoller.setStateCommand(ClawRoller.State.OFF), - m_profiledArm.setStateCommand(Arm.State.STOW)))); + // m_driver + // .leftTrigger().and(isCoralMode) + // .whileTrue( + // Commands.sequence( + // joystickDriveAtAngle( + // () -> FieldConstants.getNearestCoralStation(m_drive.getPose()) + // .getRotation()), + // m_profiledElevator.setStateCommand(Elevator.State.CORAL_INTAKE), + // Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) + // .andThen(Commands.parallel( + // m_profiledArm.setStateCommand(Arm.State.CORAL_INTAKE), + // m_clawRoller.setStateCommand(ClawRoller.State.INTAKE)), + // Commands.waitUntil(m_clawRollerLaserCAN.triggered) + // .andThen(m_clawRoller.setStateCommand(ClawRoller.State.HOLDCORAL)))) + // .andThen( + // Commands.parallel( + // m_clawRoller.setStateCommand(ClawRoller.State.OFF), + // m_profiledArm.setStateCommand(Arm.State.STOW)))); // Driver Left Trigger + Right Bumper: Algae Intake - m_driver.leftTrigger().and(isCoralMode.negate()).whileTrue( - Commands.sequence( - (FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT) - .getTranslation().getX() > 0) - ? m_profiledElevator.setStateCommand(Elevator.State.ALGAE_HIGH) - : m_profiledElevator.setStateCommand(Elevator.State.ALGAE_LOW), - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE), - Commands.waitUntil(m_clawRoller.stalled) - .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) - .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))); - m_driver.rightTrigger().whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.SCORE)); + // m_driver.leftTrigger().and(isCoralMode.negate()).whileTrue( + // Commands.sequence( + // (FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT) + // .getTranslation().getX() > 0) + // ? m_profiledElevator.setStateCommand(Elevator.State.ALGAE_HIGH) + // : m_profiledElevator.setStateCommand(Elevator.State.ALGAE_LOW), + // m_clawRoller.setStateCommand(ClawRoller.State.INTAKE), + // Commands.waitUntil(m_clawRoller.stalled) + // .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) + // .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))); + // m_driver.rightTrigger().whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.SCORE)); // Driver Start Button: Climb Request (toggle) - m_driver.start().onTrue(Commands.runOnce(() -> { - m_profiledClimber.climbRequested = true; - m_profiledClimber.climbStep += 1; - })); + // m_driver.start().onTrue(Commands.runOnce(() -> { + // m_profiledClimber.climbRequested = true; + // m_profiledClimber.climbStep += 1; + // })); // Climb step 1: Get the Arm Down, then the Elevator down, and then and move climber to prep - m_profiledClimber.getClimbRequest().and(m_profiledClimber.getClimbStep1()).whileTrue( - Commands.parallel( - Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.CLIMB), - Commands.waitUntil(() -> m_profiledArm.atPosition(0.1)) - .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))), - Commands - .waitUntil( - () -> m_profiledElevator.atPosition(0.1) && m_profiledArm.atPosition(0.1))) - .andThen(m_profiledClimber.setStateCommand(Climber.State.PREP))); + // m_profiledClimber.getClimbRequest().and(m_profiledClimber.getClimbStep1()).whileTrue( + // Commands.parallel( + // Commands.parallel( + // m_profiledArm.setStateCommand(Arm.State.CLIMB), + // Commands.waitUntil(() -> m_profiledArm.atPosition(0.1)) + // .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))), + // Commands + // .waitUntil( + // () -> m_profiledElevator.atPosition(0.1) && m_profiledArm.atPosition(0.1))) + // .andThen(m_profiledClimber.setStateCommand(Climber.State.PREP))); // Climb step 2: Move climber to climb - m_profiledClimber.getClimbRequest().and(m_profiledClimber.getClimbStep2()) - .whileTrue( - m_profiledClimber.setStateCommand(Climber.State.CLIMB) - .until(m_profiledClimber.getClimbedTrigger())); + // m_profiledClimber.getClimbRequest().and(m_profiledClimber.getClimbStep2()) + // .whileTrue( + // m_profiledClimber.setStateCommand(Climber.State.CLIMB) + // .until(m_profiledClimber.getClimbedTrigger())); - m_profiledClimber.getClimbedTrigger().onTrue(m_profiledClimber.climbedAlertCommand()); + // m_profiledClimber.getClimbedTrigger().onTrue(m_profiledClimber.climbedAlertCommand()); // Driver POV Right: End Climbing Sequence if needed - m_driver - .povRight() - .onTrue( - Commands.runOnce( - () -> { - m_profiledClimber.climbRequested = false; - m_profiledClimber.climbStep = 0; - })); + // m_driver + // .povRight() + // .onTrue( + // Commands.runOnce( + // () -> { + // m_profiledClimber.climbRequested = false; + // m_profiledClimber.climbStep = 0; + // })); // Slow drivetrain to 25% while climbing - m_profiledClimber.getClimbRequest().whileTrue( - DriveCommands.joystickDrive( - m_drive, - () -> -m_driver.getLeftY() * 0.25, - () -> -m_driver.getLeftX() * 0.25, - () -> -m_driver.getRightX() * 0.25)); + // m_profiledClimber.getClimbRequest().whileTrue( + // DriveCommands.joystickDrive( + // m_drive, + // () -> -m_driver.getLeftY() * 0.25, + // () -> -m_driver.getLeftX() * 0.25, + // () -> -m_driver.getRightX() * 0.25)); // Driver POV Down: Zero the Elevator (HOMING) m_driver.povDown().whileTrue( @@ -443,11 +444,11 @@ private void configureControllerBindings() // Driver Right Bumper: Toggle between Coral and Algae Modes. // Make sure the Approach nearest reef face does not mess with this - m_driver.rightBumper() - .onTrue( - !m_driver.leftBumper().getAsBoolean() ? setCoralAlgaeModeCommand() - : Commands.runOnce(() -> { - })); + // m_driver.rightBumper() + // .onTrue( + // !m_driver.leftBumper().getAsBoolean() ? setCoralAlgaeModeCommand() + // : Commands.runOnce(() -> { + // })); } diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index f3ee441..d9d8ce0 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -22,9 +22,9 @@ public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), STOW(Units.degreesToRotations(20.0), 0.0, ProfileType.MM_POSITION), CORAL_INTAKE(Units.degreesToRotations(0.0), 0.0, ProfileType.MM_POSITION), - LEVEL_1(Units.degreesToRotations(20.0), 0.0, ProfileType.MM_POSITION), - LEVEL_2(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - LEVEL_3(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), + LEVEL_1(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), + LEVEL_2(Units.degreesToRotations(70.0), 0.0, ProfileType.MM_POSITION), + LEVEL_3(Units.degreesToRotations(70.0), 0.0, ProfileType.MM_POSITION), LEVEL_4(Units.degreesToRotations(60.0), 0.0, ProfileType.MM_POSITION), CLIMB(Units.degreesToRotations(95.0), 0.0, ProfileType.MM_POSITION), ALGAE_LOW(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), diff --git a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java index 3c959bd..30396f0 100644 --- a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java @@ -49,7 +49,7 @@ public final class ArmConstants { FeedbackSensorSourceValue.RemoteCANcoder; kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 1; kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = (9 / 1) * (48 / 22) * (70 / 22); - kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.826416015625; + kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.76806640625; kSubSysConstants.kEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; kSubSysConstants.kEncoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; @@ -57,7 +57,7 @@ public final class ArmConstants { kSubSysConstants.kMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; kSubSysConstants.kMotorConfig.MotorOutput.Inverted = - InvertedValue.Clockwise_Positive; + InvertedValue.CounterClockwise_Positive; kSubSysConstants.kMotorConfig.Voltage.PeakForwardVoltage = 12.0; kSubSysConstants.kMotorConfig.Voltage.PeakReverseVoltage = -12.0; @@ -66,20 +66,22 @@ public final class ArmConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimit = 70; kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = -0.04; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.25; kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 0; + kSubSysConstants.kMotorConfig.Slot0.kP = 800; kSubSysConstants.kMotorConfig.Slot0.kI = 0; - kSubSysConstants.kMotorConfig.Slot0.kD = 0; + kSubSysConstants.kMotorConfig.Slot0.kD = 85; kSubSysConstants.kMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - kSubSysConstants.kMotorConfig.Slot0.kG = 0; - kSubSysConstants.kMotorConfig.Slot0.kS = 35; + kSubSysConstants.kMotorConfig.Slot0.kG = 12; + kSubSysConstants.kMotorConfig.Slot0.kS = 4; kSubSysConstants.kMotorConfig.Slot0.kV = 0; kSubSysConstants.kMotorConfig.Slot0.kA = 0; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 0; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 150; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 80; kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicJerk = 0; /* SIM system profile constants */ From e3c4a7b104c82320fb54d5f0a74fc642c5155001 Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Thu, 13 Feb 2025 19:55:12 -0500 Subject: [PATCH 07/24] Make subsystem state outputs double suppliers --- .../java/frc/robot/subsystems/Arm/Arm.java | 29 ++++++++-------- .../Claw/ClawRoller/ClawRoller.java | 15 +++++---- .../frc/robot/subsystems/Climber/Climber.java | 9 ++--- .../robot/subsystems/Elevator/Elevator.java | 33 ++++++++++--------- .../GenericMotionProfiledSubsystem.java | 15 +++++---- 5 files changed, 53 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index d9d8ce0..2ca9610 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.Arm; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; @@ -20,21 +21,21 @@ public class Arm extends GenericMotionProfiledSubsystem { @Getter public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), - STOW(Units.degreesToRotations(20.0), 0.0, ProfileType.MM_POSITION), - CORAL_INTAKE(Units.degreesToRotations(0.0), 0.0, ProfileType.MM_POSITION), - LEVEL_1(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - LEVEL_2(Units.degreesToRotations(70.0), 0.0, ProfileType.MM_POSITION), - LEVEL_3(Units.degreesToRotations(70.0), 0.0, ProfileType.MM_POSITION), - LEVEL_4(Units.degreesToRotations(60.0), 0.0, ProfileType.MM_POSITION), - CLIMB(Units.degreesToRotations(95.0), 0.0, ProfileType.MM_POSITION), - ALGAE_LOW(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - ALGAE_HIGH(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - ALGAE_GROUND(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION), - ALGAE_SCORE(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - BARGE(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - CHARACTERIZATION(0.0, 0.0, ProfileType.CHARACTERIZATION); + STOW(() -> Units.degreesToRotations(124.0), 0.0, ProfileType.MM_POSITION), + CORAL_INTAKE(() -> Units.degreesToRotations(144.0), 0.0, ProfileType.MM_POSITION), + LEVEL_1(() -> Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), + LEVEL_2(() -> Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), + LEVEL_3(() -> Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), + LEVEL_4(() ->Units.degreesToRotations(88.0), 0.0, ProfileType.MM_POSITION), + CLIMB(() -> Units.degreesToRotations(49.0), 0.0, ProfileType.MM_POSITION), + ALGAE_LOW(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), + ALGAE_HIGH(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), + ALGAE_GROUND(() -> Units.degreesToRotations(9.0), 0.0, ProfileType.MM_POSITION), + ALGAE_SCORE(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), + BARGE(() -> Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), + CHARACTERIZATION(() -> 0.0, 0.0, ProfileType.CHARACTERIZATION); - private final double output; + private final DoubleSupplier output; private final double feedFwd; private final ProfileType profileType; diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 22a8c71..0af1535 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.Claw.ClawRoller; +import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem; @@ -21,15 +22,15 @@ public class ClawRoller @RequiredArgsConstructor @Getter public enum State implements TargetState { - OFF(0.0, 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot - INTAKE(6.0, 0.0, ProfileType.OPEN_VOLTAGE), - EJECT(-6.0, 0.0, ProfileType.OPEN_VOLTAGE), - SCORE(6.0, 0.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(1.0, 0.0, ProfileType.MM_POSITION); // One rotation after detecting coral, switch + OFF(() -> 0.0, 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot + INTAKE(() -> 6.0, 0.0, ProfileType.OPEN_VOLTAGE), + EJECT(() -> 6.0, 0.0, ProfileType.OPEN_VOLTAGE), + SCORE(() -> 6.0, 0.0, ProfileType.OPEN_VOLTAGE), + HOLDCORAL(() -> 1.0, 0.0, ProfileType.MM_POSITION); // One rotation after detecting coral, switch // to HOLDCORAL to tell the motors to go one // rotaiton - private final double output; + private final DoubleSupplier output; private final double feedFwd; private final ProfileType profileType; } @@ -55,6 +56,6 @@ public Command setStateCommandNoEnd(State state) public boolean atPosition(double tolerance) { - return Util.epsilonEquals(io.getPosition(), state.output, Math.max(0.0001, tolerance)); + return Util.epsilonEquals(io.getPosition(), state.output.getAsDouble(), Math.max(0.0001, tolerance)); } } diff --git a/src/main/java/frc/robot/subsystems/Climber/Climber.java b/src/main/java/frc/robot/subsystems/Climber/Climber.java index 4a5b8ca..aa9a4b9 100644 --- a/src/main/java/frc/robot/subsystems/Climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber/Climber.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.Climber; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.util.Units; @@ -25,11 +26,11 @@ public class Climber extends GenericMotionProfiledSubsystem { @Getter public enum State implements TargetState { // HOME is climber upright, Prep - Assuming that PREP position is parallel to the x axis, CLIMB is inwards - HOME(Units.degreesToRotations(90), 0.0, ProfileType.MM_POSITION), - PREP(Units.degreesToRotations(0.0), 0.0, ProfileType.MM_POSITION), - CLIMB(Units.degreesToRotations(110.0), 0.0, ProfileType.MM_POSITION); + HOME(() -> Units.degreesToRotations(90), 0.0, ProfileType.MM_POSITION), + PREP(() -> Units.degreesToRotations(0.0), 0.0, ProfileType.MM_POSITION), + CLIMB(() -> Units.degreesToRotations(110.0), 0.0, ProfileType.MM_POSITION); - private final double output; + private final DoubleSupplier output; private final double feedFwd; private final ProfileType profileType; } diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java index 86bb210..f0f34f4 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.Elevator; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; @@ -24,23 +25,23 @@ public class Elevator extends GenericMotionProfiledSubsystem { @RequiredArgsConstructor @Getter public enum State implements TargetState { - HOMING(-0.2, 0.0, ProfileType.OPEN_VOLTAGE), + HOMING(() -> -0.2, 0.0, ProfileType.OPEN_VOLTAGE), // TODO: Test Voltage and position values (rotations) - STOW(0.15, 0.0, ProfileType.MM_POSITION), - CORAL_INTAKE(0.05, 0.0, ProfileType.MM_POSITION), - LEVEL_1(0.2, 0.0, ProfileType.MM_POSITION), - LEVEL_2(1, 0.0, ProfileType.MM_POSITION), - LEVEL_3(2, 0.0, ProfileType.MM_POSITION), - LEVEL_4(3.75, 0.0, ProfileType.MM_POSITION), - CLIMB(0.05, 0.0, ProfileType.MM_POSITION), - ALGAE_LOW(0.8, 0.0, ProfileType.MM_POSITION), - ALGAE_HIGH(1.5, 0.0, ProfileType.MM_POSITION), - ALGAE_GROUND(0.05, 0.0, ProfileType.MM_POSITION), - ALGAE_SCORE(0.05, 0.0, ProfileType.MM_POSITION), - BARGE(4.0, 0.0, ProfileType.MM_POSITION), - CHARACTERIZATION(0.0, 0.0, ProfileType.CHARACTERIZATION); - - private final double output; + STOW(() -> 0.15, 0.0, ProfileType.MM_POSITION), + CORAL_INTAKE(() -> 0.05, 0.0, ProfileType.MM_POSITION), + LEVEL_1(() -> new LoggedTunableNumber("Elevator/L1SP", 0.2).getAsDouble(), 0.0, ProfileType.MM_POSITION), + LEVEL_2(() -> new LoggedTunableNumber("Elevator/L2SP", 1.5).getAsDouble(), 0.0, ProfileType.MM_POSITION), + LEVEL_3(() -> new LoggedTunableNumber("Elevator/L3SP", 2.8).getAsDouble(), 0.0, ProfileType.MM_POSITION), + LEVEL_4(() -> new LoggedTunableNumber("Elevator/L4SP", 5.2).getAsDouble(), 0.0, ProfileType.MM_POSITION), + CLIMB(() -> 0.05, 0.0, ProfileType.MM_POSITION), + ALGAE_LOW(() -> 0.8, 0.0, ProfileType.MM_POSITION), + ALGAE_HIGH(() -> 1.5, 0.0, ProfileType.MM_POSITION), + ALGAE_GROUND(() -> 0.05, 0.0, ProfileType.MM_POSITION), + ALGAE_SCORE(() -> 0.05, 0.0, ProfileType.MM_POSITION), + BARGE(() -> 4.0, 0.0, ProfileType.MM_POSITION), + CHARACTERIZATION(() -> 0.0, 0.0, ProfileType.CHARACTERIZATION); + + private final DoubleSupplier output; private final double feedFwd; private final ProfileType profileType; } diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java index a2c8928..c72adc2 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTunableNumber; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; public abstract class GenericMotionProfiledSubsystem @@ -24,7 +25,7 @@ public enum ProfileType { } public interface TargetState { - public double getOutput(); + public DoubleSupplier getOutput(); public double getFeedFwd(); @@ -133,27 +134,27 @@ public void periodic() default: case POSITION: /* Run Closed Loop to position in rotations */ - io.runToPosition(getState().getOutput(), getState().getFeedFwd()); + io.runToPosition(getState().getOutput().getAsDouble(), getState().getFeedFwd()); break; case VELOCITY: /* Run Closed Loop to velocity in rotations/second */ - io.runToVelocity(getState().getOutput(), getState().getFeedFwd()); + io.runToVelocity(getState().getOutput().getAsDouble(), getState().getFeedFwd()); break; case MM_POSITION: /* Run Motion Magic to the specified position setpoint (in rotations) */ - io.runMotionMagicPosition(getState().getOutput(), getState().getFeedFwd()); + io.runMotionMagicPosition(getState().getOutput().getAsDouble(), getState().getFeedFwd()); break; case MM_VELOCITY: /* Run Motion Magic to the specified velocity setpoint (in rotations/second) */ - io.runMotionMagicVelocity(getState().getOutput(), getState().getFeedFwd()); + io.runMotionMagicVelocity(getState().getOutput().getAsDouble(), getState().getFeedFwd()); break; case OPEN_VOLTAGE: /* Run Open Loop using specified voltage in volts */ - io.runVoltage(getState().getOutput()); + io.runVoltage(getState().getOutput().getAsDouble()); break; case OPEN_CURRENT: /* Run Open Loop using specified current in amps */ - io.runCurrent(getState().getOutput()); + io.runCurrent(getState().getOutput().getAsDouble()); break; case CHARACTERIZATION: /* From 03934493ea77abf4a640b3c798ddad682c1a951b Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Thu, 13 Feb 2025 21:13:19 -0500 Subject: [PATCH 08/24] arm logged tunable numbers --- src/main/java/frc/robot/subsystems/Arm/Arm.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index 2ca9610..710f89f 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -21,12 +21,12 @@ public class Arm extends GenericMotionProfiledSubsystem { @Getter public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), - STOW(() -> Units.degreesToRotations(124.0), 0.0, ProfileType.MM_POSITION), + STOW(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/StowSP", 124.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), CORAL_INTAKE(() -> Units.degreesToRotations(144.0), 0.0, ProfileType.MM_POSITION), - LEVEL_1(() -> Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), - LEVEL_2(() -> Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), - LEVEL_3(() -> Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), - LEVEL_4(() ->Units.degreesToRotations(88.0), 0.0, ProfileType.MM_POSITION), + LEVEL_1(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L1SP", 120.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), + LEVEL_2(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L2SP", 105.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), + LEVEL_3(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L3SP", 105.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), + LEVEL_4(() ->Units.degreesToRotations(new LoggedTunableNumber("Arm/L4SP", 90.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), CLIMB(() -> Units.degreesToRotations(49.0), 0.0, ProfileType.MM_POSITION), ALGAE_LOW(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), ALGAE_HIGH(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), From b1442b7d43ac76b06e73c0d17e4ce4deac4c09cd Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Thu, 13 Feb 2025 21:28:03 -0500 Subject: [PATCH 09/24] 2/13 --- ascope_assets/Robot_Alpha/config.json | 14 +++++------ src/main/java/frc/robot/Ports.java | 3 +++ src/main/java/frc/robot/RobotContainer.java | 20 ++++++++++++--- .../java/frc/robot/subsystems/Arm/Arm.java | 25 +++++++++++-------- .../robot/subsystems/Arm/ArmConstants.java | 4 +-- .../Claw/ClawRoller/ClawRoller.java | 8 +++--- .../Claw/ClawRoller/ClawRollerConstants.java | 8 +++--- .../ClawRollerLaserCAN.java | 2 +- .../Claw/IntakeLaserCAN/IntakeLaserCAN.java | 2 +- .../robot/subsystems/Elevator/Elevator.java | 8 +++--- .../frc/robot/subsystems/Superstructure.java | 8 +++--- .../subsystems/Vision/VisionConstants.java | 5 ++-- 12 files changed, 64 insertions(+), 43 deletions(-) diff --git a/ascope_assets/Robot_Alpha/config.json b/ascope_assets/Robot_Alpha/config.json index 3736882..ac8b976 100644 --- a/ascope_assets/Robot_Alpha/config.json +++ b/ascope_assets/Robot_Alpha/config.json @@ -26,9 +26,9 @@ 1304 ], "position": [ - 0.287, - 0.260, - 0.211 + 0.289591, + 0.261649, + 0.205829 ], "rotations": [ { @@ -45,9 +45,9 @@ 1304 ], "position": [ - 0.287, - -0.260, - 0.211 + 0.289591, + -0.261649, + 0.205829 ], "rotations": [ { @@ -114,4 +114,4 @@ ] } ] -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index cae6cdd..8dda3e5 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -13,6 +13,7 @@ public class Ports { public static final CanDeviceId CLAW_LASERCAN = new CanDeviceId(16, "rio"); public static final CanDeviceId CLAW_ROLLER = new CanDeviceId(17, "rio"); + public static final CanDeviceId ARM_CANCODER = new CanDeviceId(18, "rio"); public static final CanDeviceId ARM_MAIN = new CanDeviceId(19, "rio"); @@ -25,4 +26,6 @@ public class Ports { public static final CanDeviceId CLIMBER = new CanDeviceId(24, "Drivetrain"); + public static final CanDeviceId PDH = new CanDeviceId(25, "rio"); + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9f3d0ee..fcb5e0a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,8 +102,8 @@ public RobotContainer() // (robotPose) -> { // }); - m_profiledArm = new Arm(new ArmIOTalonFX(), false); - m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); + // m_profiledArm = new Arm(new ArmIOTalonFX(), false); + // m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); // m_profiledClimber = new Climber(new ClimberIOTalonFX(), false); // m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIOReal()); @@ -127,8 +127,8 @@ public RobotContainer() (robotPose) -> { }); - // m_profiledArm = new Arm(new ArmIO() {}, true); - // m_profiledElevator = new Elevator(new ElevatorIO() {}, true); + m_profiledArm = new Arm(new ArmIO() {}, true); + m_profiledElevator = new Elevator(new ElevatorIO() {}, true); m_profiledClimber = new Climber(new ClimberIO() {}, true); m_clawRoller = new ClawRoller(new ClawRollerIO() {}, true); // m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIO() {}); @@ -358,8 +358,20 @@ private void configureControllerBindings() m_driver.rightTrigger() .whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.EJECT)); + + // Driver Left Trigger: Drivetrain drive at coral station angle, prepare the elevator and // arm, Get Ready to Intake Coral + m_driver + .leftTrigger() + .whileTrue( + Commands.sequence( + m_profiledElevator.setStateCommand(Elevator.State.CORAL_INTAKE), + Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) + .andThen(Commands.parallel( + m_profiledArm.setStateCommand(Arm.State.CORAL_INTAKE), + m_clawRoller.setStateCommand(ClawRoller.State.INTAKE)) + .until(m_clawRollerLaserCAN.triggered)))); // m_driver // .leftTrigger().and(isCoralMode) // .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index d9d8ce0..9767709 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -16,21 +16,24 @@ @Getter public class Arm extends GenericMotionProfiledSubsystem { + // .14 rot is the max extension + @RequiredArgsConstructor @Getter public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), - STOW(Units.degreesToRotations(20.0), 0.0, ProfileType.MM_POSITION), - CORAL_INTAKE(Units.degreesToRotations(0.0), 0.0, ProfileType.MM_POSITION), - LEVEL_1(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - LEVEL_2(Units.degreesToRotations(70.0), 0.0, ProfileType.MM_POSITION), - LEVEL_3(Units.degreesToRotations(70.0), 0.0, ProfileType.MM_POSITION), - LEVEL_4(Units.degreesToRotations(60.0), 0.0, ProfileType.MM_POSITION), - CLIMB(Units.degreesToRotations(95.0), 0.0, ProfileType.MM_POSITION), - ALGAE_LOW(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - ALGAE_HIGH(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - ALGAE_GROUND(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION), - ALGAE_SCORE(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), + STOW(Units.degreesToRotations(124.0), 0.0, ProfileType.MM_POSITION), + + CORAL_INTAKE(Units.degreesToRotations(144.0), 0.0, ProfileType.MM_POSITION), + LEVEL_1(Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), + LEVEL_2(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION), + LEVEL_3(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION), + LEVEL_4(Units.degreesToRotations(90.0), 0.0, ProfileType.MM_POSITION), + CLIMB(.14, 0.0, ProfileType.MM_POSITION), + ALGAE_LOW(Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), + ALGAE_HIGH(Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), + ALGAE_GROUND(Units.degreesToRotations(9.0), 0.0, ProfileType.MM_POSITION), + ALGAE_SCORE(Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), BARGE(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), CHARACTERIZATION(0.0, 0.0, ProfileType.CHARACTERIZATION); diff --git a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java index 30396f0..7183952 100644 --- a/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/Arm/ArmConstants.java @@ -49,7 +49,7 @@ public final class ArmConstants { FeedbackSensorSourceValue.RemoteCANcoder; kSubSysConstants.kMotorConfig.Feedback.SensorToMechanismRatio = 1; kSubSysConstants.kMotorConfig.Feedback.RotorToSensorRatio = (9 / 1) * (48 / 22) * (70 / 22); - kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.76806640625; + kSubSysConstants.kEncoderConfig.MagnetSensor.MagnetOffset = 0.826416015625; kSubSysConstants.kEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; kSubSysConstants.kEncoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1; @@ -66,7 +66,7 @@ public final class ArmConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimit = 70; kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.25; + kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.405; kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; kSubSysConstants.kMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 22a8c71..a63092c 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -22,10 +22,10 @@ public class ClawRoller @Getter public enum State implements TargetState { OFF(0.0, 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot - INTAKE(6.0, 0.0, ProfileType.OPEN_VOLTAGE), - EJECT(-6.0, 0.0, ProfileType.OPEN_VOLTAGE), - SCORE(6.0, 0.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(1.0, 0.0, ProfileType.MM_POSITION); // One rotation after detecting coral, switch + INTAKE(2.0, 0.0, ProfileType.OPEN_VOLTAGE), + EJECT(6.0, 0.0, ProfileType.OPEN_VOLTAGE), + SCORE(8.0, 0.0, ProfileType.OPEN_VOLTAGE), + HOLDCORAL(0.0, 0.0, ProfileType.MM_POSITION); // One rotation after detecting coral, switch // to HOLDCORAL to tell the motors to go one // rotaiton diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java index ce206bf..a6fa950 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java @@ -41,15 +41,15 @@ public final class ClawRollerConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 0; // TODO: tune on real robot + kSubSysConstants.kMotorConfig.Slot0.kP = 1.0; // TODO: tune on real robot kSubSysConstants.kMotorConfig.Slot0.kI = 0; kSubSysConstants.kMotorConfig.Slot0.kD = 0; kSubSysConstants.kMotorConfig.Slot0.kG = 0; - kSubSysConstants.kMotorConfig.Slot0.kS = 0; + kSubSysConstants.kMotorConfig.Slot0.kS = 36; kSubSysConstants.kMotorConfig.Slot0.kV = 0; kSubSysConstants.kMotorConfig.Slot0.kA = 0; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0; - kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 0; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 100; + kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicAcceleration = 10; kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicJerk = 0; /* SIM system profile constants */ diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCAN.java b/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCAN.java index eecaddf..0f7142b 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCAN.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCAN.java @@ -16,7 +16,7 @@ public class ClawRollerLaserCAN extends GenericLaserCANSubsystem m_Arm.atPosition(armTolerance)), // Move Elevator to new position - m_Elevator.setStateCommand(elevatorState) - .until(() -> m_Elevator.atPosition(elevTolerance)), + Commands.waitUntil(() -> m_Arm.atPosition(armTolerance)) + .andThen(m_Elevator.setStateCommand(elevatorState) + .until(() -> m_Elevator.atPosition(elevTolerance))), // Reposition Arm to new position - m_Arm.setStateCommand(armState).until(() -> m_Arm.atPosition(armTolerance)))); + Commands.waitUntil(() -> m_Elevator.atPosition(elevTolerance)).andThen( + m_Arm.setStateCommand(armState).until(() -> m_Arm.atPosition(armTolerance))))); } /** diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java index 91c2130..e326c6f 100644 --- a/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java @@ -31,9 +31,10 @@ public class VisionConstants { // Robot to camera transforms // (Not used by Limelight, configure in web UI instead) public static Transform3d robotToCamera0 = - new Transform3d(0.287, 0.260, 0.211, new Rotation3d(0.0, Units.degreesToRadians(-15), 0.0)); + new Transform3d(0.289591, 0.261649, 0.205829, + new Rotation3d(0.0, Units.degreesToRadians(-15), 0.0)); public static Transform3d robotToCamera1 = - new Transform3d(0.287, -0.260, 0.211, + new Transform3d(0.289591, -0.261649, 0.205829, new Rotation3d(0.0, Units.degreesToRadians(-15), 0.0)); // Basic filtering thresholds From 9ba416677744650f6d23899298177e174af6b478 Mon Sep 17 00:00:00 2001 From: spybh66 Date: Fri, 14 Feb 2025 13:23:24 -0500 Subject: [PATCH 10/24] updated wpilib, ak, grapple, maple, pp, phoenix --- build.gradle | 2 +- vendordeps/AdvantageKit.json | 68 +-- ....2.2.json => PathplannerLib-2025.2.3.json} | 8 +- vendordeps/Phoenix6-25.2.1.json | 419 ------------------ vendordeps/Phoenix6-25.2.2.json | 419 ++++++++++++++++++ vendordeps/libgrapplefrc2025.json | 146 +++--- vendordeps/maple-sim.json | 4 +- 7 files changed, 533 insertions(+), 533 deletions(-) rename vendordeps/{PathplannerLib-2025.2.2.json => PathplannerLib-2025.2.3.json} (87%) delete mode 100644 vendordeps/Phoenix6-25.2.1.json create mode 100644 vendordeps/Phoenix6-25.2.2.json diff --git a/build.gradle b/build.gradle index c282c24..2781270 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" id "io.freefair.lombok" version "8.6" diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index fa81b2f..c587313 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,35 +1,35 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "4.1.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2025", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-java", - "version": "4.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-wpilibio", - "version": "4.1.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [] -} + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.2.3.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.2.json rename to vendordeps/PathplannerLib-2025.2.3.json index a5bf9ee..9151ce4 100644 --- a/vendordeps/PathplannerLib-2025.2.2.json +++ b/vendordeps/PathplannerLib-2025.2.3.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.2.json", + "fileName": "PathplannerLib-2025.2.3.json", "name": "PathplannerLib", - "version": "2025.2.2", + "version": "2025.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.2" + "version": "2025.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.2", + "version": "2025.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.2.1.json deleted file mode 100644 index 349f124..0000000 --- a/vendordeps/Phoenix6-25.2.1.json +++ /dev/null @@ -1,419 +0,0 @@ -{ - "fileName": "Phoenix6-25.2.1.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} diff --git a/vendordeps/Phoenix6-25.2.2.json b/vendordeps/Phoenix6-25.2.2.json new file mode 100644 index 0000000..d617643 --- /dev/null +++ b/vendordeps/Phoenix6-25.2.2.json @@ -0,0 +1,419 @@ +{ + "fileName": "Phoenix6-25.2.2.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.2.2", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.2.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.2.2", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.2.2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.2.2", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.2.2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.2.2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.2.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.2.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.2.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.2.2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.2", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.2.2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.2.2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.2", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json index a711280..f593245 100644 --- a/vendordeps/libgrapplefrc2025.json +++ b/vendordeps/libgrapplefrc2025.json @@ -1,74 +1,74 @@ { - "fileName": "libgrapplefrc2025.json", - "name": "libgrapplefrc", - "version": "2025.0.8", - "frcYear": "2025", - "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", - "mavenUrls": [ - "https://storage.googleapis.com/grapple-frc-maven" - ], - "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", - "javaDependencies": [ - { - "groupId": "au.grapplerobotics", - "artifactId": "libgrapplefrcjava", - "version": "2025.0.8" - } - ], - "jniDependencies": [ - { - "groupId": "au.grapplerobotics", - "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "au.grapplerobotics", - "artifactId": "libgrapplefrccpp", - "version": "2025.0.8", - "libName": "grapplefrc", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "au.grapplerobotics", - "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", - "libName": "grapplefrcdriver", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.1.0", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.1.0", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.1.0", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index a85f5c1..df5da73 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.6", + "version": "0.3.9", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.6" + "version": "0.3.9" }, { "groupId": "org.dyn4j", From f81440e0da1af137d9297ad1da8310faa74d5649 Mon Sep 17 00:00:00 2001 From: spybh66 Date: Fri, 14 Feb 2025 17:12:40 -0500 Subject: [PATCH 11/24] red alliance reef positions work, drive pid doesn't --- src/main/java/frc/robot/FieldConstants.java | 46 ++++++++++++++++--- src/main/java/frc/robot/RobotContainer.java | 6 +-- .../frc/robot/commands/DriveCommands.java | 7 ++- 3 files changed, 48 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index cc41c8e..47fe4ac 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -13,6 +13,7 @@ import java.util.HashMap; import java.util.List; import java.util.Map; +import org.littletonrobotics.junction.Logger; /** * Contains various field dimensions and useful reference points. All units are in meters and poses @@ -21,6 +22,8 @@ public class FieldConstants { public static final double fieldLength = Units.inchesToMeters(690.876); public static final double fieldWidth = Units.inchesToMeters(317); + public static final Translation2d fieldCenter = + new Translation2d(fieldLength / 2, fieldWidth / 2); public static final double startingLineX = Units.inchesToMeters(299.438); // Measured from the inside of starting // line @@ -57,14 +60,14 @@ public static class CoralStation { } public static class Reef { - public static final Translation2d center = + public static final Translation2d centerOfReef = new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); public static final double faceToZoneLine = Units.inchesToMeters(12); // Side of the reef to the inside of the // reef zone line public static final Pose2d[] centerFaces = - new Pose2d[6]; // Starting facing the driver station in clockwise + new Pose2d[12]; // Starting facing the driver station in clockwise // order public static final List> branchPositions = new ArrayList<>(); // Starting at the right @@ -105,15 +108,31 @@ public static class Reef { Units.inchesToMeters(130.144), Rotation2d.fromDegrees(-120)); + centerFaces[6] = centerFaces[0].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[7] = centerFaces[1].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[8] = centerFaces[2].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[9] = centerFaces[3].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[10] = centerFaces[4].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[11] = centerFaces[5].rotateAround(fieldCenter, Rotation2d.k180deg); + // Initialize branch positions - for (int face = 0; face < 6; face++) { + for (int face = 0; face < centerFaces.length; face++) { Map fillRight = new HashMap<>(); Map fillLeft = new HashMap<>(); for (var level : ReefHeight.values()) { - Pose2d poseDirection = - new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face))); - double adjustX = Units.inchesToMeters(30.738); - double adjustY = Units.inchesToMeters(6.469); + Pose2d poseDirection = new Pose2d(); + if (face < 6) { + poseDirection = + new Pose2d(centerOfReef, centerFaces[face].getRotation()); + } else { + poseDirection = + new Pose2d(centerOfReef.rotateAround(fieldCenter, Rotation2d.k180deg), + centerFaces[face].getRotation()); + } + + double adjustX = Units.inchesToMeters(30.738); // Depth of branch from reef face + double adjustY = Units.inchesToMeters(6.469); // Offset from reef face + // centerline to branch fillRight.put( level, @@ -149,10 +168,16 @@ public static class Reef { 0, Units.degreesToRadians(level.pitch), poseDirection.getRotation().getRadians()))); + Logger.recordOutput("AlignmentDebug/LastBranchL", + fillRight.values().toArray(new Pose3d[0])); } branchPositions.add(fillRight); branchPositions.add(fillLeft); + + Logger.recordOutput("AlignmentDebug/CenterFaces", centerFaces); } + + } } @@ -184,6 +209,8 @@ public enum ReefHeight { public static Pose2d getNearestReefFace(Pose2d currentPose) { + Logger.recordOutput("AlignmentDebug/ClosestReefFace", + currentPose.nearest(List.of(FieldConstants.Reef.centerFaces))); return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces)); } @@ -194,6 +221,11 @@ public enum ReefSide { public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side) { + Logger.recordOutput("AlignmentDebug/ClosestBranch", FieldConstants.Reef.branchPositions + .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) + * 2 + (side == ReefSide.LEFT ? 1 : 0)) + .get(FieldConstants.ReefHeight.L1).toPose2d()); + return FieldConstants.Reef.branchPositions .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) * 2 + (side == ReefSide.LEFT ? 1 : 0)) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62c9d08..e21031a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,7 +75,7 @@ public class RobotContainer { private final Superstructure m_superStruct; public final Vision m_vision; - public final LED m_LED; + // public final LED m_LED; // Trigger for algae/coral mode switching private boolean coralModeEnabled = false; @@ -182,8 +182,8 @@ public RobotContainer() m_superStruct = new Superstructure(m_profiledArm, m_profiledElevator); // LED subsystem reads status from all other subsystems to control LEDs via CANdle - m_LED = new LED(m_driver, m_profiledArm, m_clawRoller, m_profiledClimber, m_drive, - m_profiledElevator, m_vision, m_clawRollerLaserCAN, isCoralMode); + // m_LED = new LED(m_driver, m_profiledArm, m_clawRoller, m_profiledClimber, m_drive, + // m_profiledElevator, m_vision, m_clawRollerLaserCAN, isCoralMode); // Logic Triggers registerNamedCommands(); diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index b31fc6c..72ff42c 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -149,10 +149,15 @@ public static Command joystickDriveAtAngle( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega); + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, - drive.getRotation())); + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive) From 19cbd853d3a1cbf778d29578e91d57a75a3cc40a Mon Sep 17 00:00:00 2001 From: Bryson Halsey Date: Fri, 14 Feb 2025 20:34:15 -0500 Subject: [PATCH 12/24] added coral station targeting --- src/main/java/frc/robot/FieldConstants.java | 23 ++++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 47fe4ac..8675440 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -234,15 +234,22 @@ public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side) public static Pose2d getNearestCoralStation(Pose2d currentPose) { - double distanceToLeftStation = currentPose.getTranslation() - .getDistance(FieldConstants.CoralStation.leftCenterFace.getTranslation()); - double distanceToRightStation = currentPose.getTranslation() - .getDistance(FieldConstants.CoralStation.rightCenterFace.getTranslation()); - - if (distanceToLeftStation > distanceToRightStation) { - return FieldConstants.CoralStation.rightCenterFace; + if (currentPose.getTranslation().getX() > FieldConstants.fieldLength / 2) { + if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { + return FieldConstants.CoralStation.rightCenterFace + .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); + } else { + return FieldConstants.CoralStation.leftCenterFace + .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); + } } else { - return FieldConstants.CoralStation.leftCenterFace; + if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { + return FieldConstants.CoralStation.leftCenterFace; + } else { + return FieldConstants.CoralStation.rightCenterFace; + } } + + } } From 247cc378a369c42136b017f6fa3b331b132dc6d2 Mon Sep 17 00:00:00 2001 From: Bryson Halsey Date: Fri, 14 Feb 2025 20:36:35 -0500 Subject: [PATCH 13/24] removed extra logging --- src/main/java/frc/robot/FieldConstants.java | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 8675440..3c0df25 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -13,7 +13,6 @@ import java.util.HashMap; import java.util.List; import java.util.Map; -import org.littletonrobotics.junction.Logger; /** * Contains various field dimensions and useful reference points. All units are in meters and poses @@ -168,13 +167,9 @@ public static class Reef { 0, Units.degreesToRadians(level.pitch), poseDirection.getRotation().getRadians()))); - Logger.recordOutput("AlignmentDebug/LastBranchL", - fillRight.values().toArray(new Pose3d[0])); } branchPositions.add(fillRight); branchPositions.add(fillLeft); - - Logger.recordOutput("AlignmentDebug/CenterFaces", centerFaces); } @@ -209,8 +204,6 @@ public enum ReefHeight { public static Pose2d getNearestReefFace(Pose2d currentPose) { - Logger.recordOutput("AlignmentDebug/ClosestReefFace", - currentPose.nearest(List.of(FieldConstants.Reef.centerFaces))); return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces)); } @@ -221,11 +214,6 @@ public enum ReefSide { public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side) { - Logger.recordOutput("AlignmentDebug/ClosestBranch", FieldConstants.Reef.branchPositions - .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) - * 2 + (side == ReefSide.LEFT ? 1 : 0)) - .get(FieldConstants.ReefHeight.L1).toPose2d()); - return FieldConstants.Reef.branchPositions .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) * 2 + (side == ReefSide.LEFT ? 1 : 0)) @@ -249,7 +237,5 @@ public static Pose2d getNearestCoralStation(Pose2d currentPose) return FieldConstants.CoralStation.rightCenterFace; } } - - } } From df74d0e20c183847c088b8e6c85dbed11feccda3 Mon Sep 17 00:00:00 2001 From: Bryson Halsey Date: Fri, 14 Feb 2025 21:15:40 -0500 Subject: [PATCH 14/24] automatically retry configuring lasercan --- .../GenericLaserCANSubsystemIOImpl.java | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/GenericLaserCANSubsystemIOImpl.java b/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/GenericLaserCANSubsystemIOImpl.java index e7ac4a8..787e9c2 100644 --- a/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/GenericLaserCANSubsystemIOImpl.java +++ b/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/GenericLaserCANSubsystemIOImpl.java @@ -18,6 +18,8 @@ public class GenericLaserCANSubsystemIOImpl implements GenericLaserCANSubsystemI private LaserCanInterface lc = null; private String name; + private int tries = 0; + private boolean hasConfiged = false; private Distance currentDistance; @@ -37,17 +39,22 @@ public GenericLaserCANSubsystemIOImpl( lc = isSim ? new LaserCANSim(name) : new LaserCan(constants.laserCANDeviceId.getDeviceNumber()); - - try { - lc.setRangingMode(constants.rangingMode); - lc.setRegionOfInterest(constants.regionOfInterest); - lc.setTimingBudget(constants.timingBudget); - failedConfig.set(false); - } catch (ConfigurationFailedException e) { - System.out.println("Configuration failed! " + e); - failedConfig.setText("Failed to configure " + name + "!"); - failedConfig.set(true); + while (!hasConfiged && tries < 5) { + try { + lc.setRangingMode(constants.rangingMode); + lc.setRegionOfInterest(constants.regionOfInterest); + lc.setTimingBudget(constants.timingBudget); + failedConfig.set(false); + System.out.println("Succesfully configured " + name); + hasConfiged = true; + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed for " + name + "! " + e); + failedConfig.setText("Failed to configure " + name + "!"); + failedConfig.set(true); + tries++; + } } + } public Distance getMeasurement() From f2cd77c7c4741699370525829904667fde8de6d4 Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Fri, 14 Feb 2025 22:48:10 -0500 Subject: [PATCH 15/24] 2/14 refactoring --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/Arm/Arm.java | 29 +++++++++---------- .../Claw/ClawRoller/ClawRoller.java | 14 ++++----- .../frc/robot/subsystems/Climber/Climber.java | 8 ++--- .../robot/subsystems/Elevator/Elevator.java | 29 +++++++++---------- .../GenericMotionProfiledSubsystem.java | 10 +++---- .../GenericMotionProfiledSubsystemIO.java | 8 ++--- .../GenericMotionProfiledSubsystemIOImpl.java | 16 +++++----- 8 files changed, 55 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9f3d0ee..03f49e1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -277,7 +277,7 @@ public Command setCoralAlgaeModeCommand() private void configureControllerBindings() { // Default command, normal field-relative drive - // m_drive.setDefaultCommand(joystickDrive()); + m_drive.setDefaultCommand(joystickDrive()); // Driver Back Button: Reset gyro / odometry final Runnable setPose = diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index 710f89f..835f28a 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -21,23 +21,22 @@ public class Arm extends GenericMotionProfiledSubsystem { @Getter public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), - STOW(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/StowSP", 124.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), - CORAL_INTAKE(() -> Units.degreesToRotations(144.0), 0.0, ProfileType.MM_POSITION), - LEVEL_1(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L1SP", 120.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), - LEVEL_2(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L2SP", 105.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), - LEVEL_3(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L3SP", 105.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), - LEVEL_4(() ->Units.degreesToRotations(new LoggedTunableNumber("Arm/L4SP", 90.0).getAsDouble()), 0.0, ProfileType.MM_POSITION), - CLIMB(() -> Units.degreesToRotations(49.0), 0.0, ProfileType.MM_POSITION), - ALGAE_LOW(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), - ALGAE_HIGH(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), - ALGAE_GROUND(() -> Units.degreesToRotations(9.0), 0.0, ProfileType.MM_POSITION), - ALGAE_SCORE(() -> Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), - BARGE(() -> Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - CHARACTERIZATION(() -> 0.0, 0.0, ProfileType.CHARACTERIZATION); + STOW(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/StowSP", 124.0).getAsDouble()), ProfileType.MM_POSITION), - private final DoubleSupplier output; - private final double feedFwd; + CORAL_INTAKE(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/CoralIntakeSP", 144.0).getAsDouble()), ProfileType.MM_POSITION), + LEVEL_1(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L1SP", 120.0).getAsDouble()), ProfileType.MM_POSITION), + LEVEL_2(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L2SP", 105.0).getAsDouble()), ProfileType.MM_POSITION), + LEVEL_3(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L3SP", 105.0).getAsDouble()), ProfileType.MM_POSITION), + LEVEL_4(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L4SP", 90.0).getAsDouble()), ProfileType.MM_POSITION), + CLIMB(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/ClimbSP", 50.4).getAsDouble()), ProfileType.MM_POSITION), + ALGAE_LOW(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeLowSP", 114.0).getAsDouble()), ProfileType.MM_POSITION), + ALGAE_HIGH(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeHighSP", 114.0).getAsDouble()), ProfileType.MM_POSITION), + ALGAE_GROUND(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeGroundSP", 9.0).getAsDouble()), ProfileType.MM_POSITION), + ALGAE_SCORE(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeScoreSP", 114.0).getAsDouble()), ProfileType.MM_POSITION), + BARGE(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/BargeSP", 30.0).getAsDouble()), ProfileType.MM_POSITION), + CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); + private final DoubleSupplier output; private final ProfileType profileType; } diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 0af1535..3c2a170 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem.TargetState; +import frc.robot.util.LoggedTunableNumber; import frc.robot.util.Util; import lombok.Getter; import lombok.RequiredArgsConstructor; @@ -22,16 +23,13 @@ public class ClawRoller @RequiredArgsConstructor @Getter public enum State implements TargetState { - OFF(() -> 0.0, 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot - INTAKE(() -> 6.0, 0.0, ProfileType.OPEN_VOLTAGE), - EJECT(() -> 6.0, 0.0, ProfileType.OPEN_VOLTAGE), - SCORE(() -> 6.0, 0.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(() -> 1.0, 0.0, ProfileType.MM_POSITION); // One rotation after detecting coral, switch - // to HOLDCORAL to tell the motors to go one - // rotaiton + OFF(() -> 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot + INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE), + EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), + SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), + HOLDCORAL(() -> new LoggedTunableNumber("ClawRoller/HoldCoralSP", 0.0).getAsDouble(), ProfileType.MM_POSITION); // Wheels should spin x rotations before stopping private final DoubleSupplier output; - private final double feedFwd; private final ProfileType profileType; } diff --git a/src/main/java/frc/robot/subsystems/Climber/Climber.java b/src/main/java/frc/robot/subsystems/Climber/Climber.java index aa9a4b9..c33aea8 100644 --- a/src/main/java/frc/robot/subsystems/Climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber/Climber.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem; import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem.TargetState; +import frc.robot.util.LoggedTunableNumber; import frc.robot.util.Util; import lombok.Getter; import lombok.RequiredArgsConstructor; @@ -26,12 +27,11 @@ public class Climber extends GenericMotionProfiledSubsystem { @Getter public enum State implements TargetState { // HOME is climber upright, Prep - Assuming that PREP position is parallel to the x axis, CLIMB is inwards - HOME(() -> Units.degreesToRotations(90), 0.0, ProfileType.MM_POSITION), - PREP(() -> Units.degreesToRotations(0.0), 0.0, ProfileType.MM_POSITION), - CLIMB(() -> Units.degreesToRotations(110.0), 0.0, ProfileType.MM_POSITION); + HOME(() -> Units.degreesToRotations(new LoggedTunableNumber("Climber/HomeSP", 90.0).getAsDouble()), ProfileType.MM_POSITION), + PREP(() -> Units.degreesToRotations(new LoggedTunableNumber("Climber/PrepSP", 0.0).getAsDouble()), ProfileType.MM_POSITION), + CLIMB(() -> Units.degreesToRotations(new LoggedTunableNumber("Climber/ClimbSP", 110.0).getAsDouble()), ProfileType.MM_POSITION); private final DoubleSupplier output; - private final double feedFwd; private final ProfileType profileType; } diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java index f0f34f4..bf1012e 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java @@ -25,24 +25,23 @@ public class Elevator extends GenericMotionProfiledSubsystem { @RequiredArgsConstructor @Getter public enum State implements TargetState { - HOMING(() -> -0.2, 0.0, ProfileType.OPEN_VOLTAGE), + HOMING(() -> new LoggedTunableNumber("Elevator/HomingVoltageSP", -0.2).getAsDouble(), ProfileType.OPEN_VOLTAGE), // TODO: Test Voltage and position values (rotations) - STOW(() -> 0.15, 0.0, ProfileType.MM_POSITION), - CORAL_INTAKE(() -> 0.05, 0.0, ProfileType.MM_POSITION), - LEVEL_1(() -> new LoggedTunableNumber("Elevator/L1SP", 0.2).getAsDouble(), 0.0, ProfileType.MM_POSITION), - LEVEL_2(() -> new LoggedTunableNumber("Elevator/L2SP", 1.5).getAsDouble(), 0.0, ProfileType.MM_POSITION), - LEVEL_3(() -> new LoggedTunableNumber("Elevator/L3SP", 2.8).getAsDouble(), 0.0, ProfileType.MM_POSITION), - LEVEL_4(() -> new LoggedTunableNumber("Elevator/L4SP", 5.2).getAsDouble(), 0.0, ProfileType.MM_POSITION), - CLIMB(() -> 0.05, 0.0, ProfileType.MM_POSITION), - ALGAE_LOW(() -> 0.8, 0.0, ProfileType.MM_POSITION), - ALGAE_HIGH(() -> 1.5, 0.0, ProfileType.MM_POSITION), - ALGAE_GROUND(() -> 0.05, 0.0, ProfileType.MM_POSITION), - ALGAE_SCORE(() -> 0.05, 0.0, ProfileType.MM_POSITION), - BARGE(() -> 4.0, 0.0, ProfileType.MM_POSITION), - CHARACTERIZATION(() -> 0.0, 0.0, ProfileType.CHARACTERIZATION); + STOW(() -> new LoggedTunableNumber("Elevator/StowSP", 0.15).getAsDouble(), ProfileType.MM_POSITION), + CORAL_INTAKE(() -> new LoggedTunableNumber("Elevator/CoralIntakeSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), + LEVEL_1(() -> new LoggedTunableNumber("Elevator/L1SP", 0.2).getAsDouble(), ProfileType.MM_POSITION), + LEVEL_2(() -> new LoggedTunableNumber("Elevator/L2SP", 1.4).getAsDouble(), ProfileType.MM_POSITION), + LEVEL_3(() -> new LoggedTunableNumber("Elevator/L3SP", 2.85).getAsDouble(), ProfileType.MM_POSITION), + LEVEL_4(() -> new LoggedTunableNumber("Elevator/L4SP", 5.2).getAsDouble(), ProfileType.MM_POSITION), + CLIMB(() -> new LoggedTunableNumber("Elevator/ClimbSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), + ALGAE_LOW(() -> new LoggedTunableNumber("Elevator/AlgaeLowSP", 0.8).getAsDouble(), ProfileType.MM_POSITION), + ALGAE_HIGH(() -> new LoggedTunableNumber("Elevator/AlgaeHighSP", 1.5).getAsDouble(), ProfileType.MM_POSITION), + ALGAE_GROUND(() -> new LoggedTunableNumber("Elevator/AlgaeGroundSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), + ALGAE_SCORE(() -> new LoggedTunableNumber("Elevator/AlgaeScoreSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), + BARGE(() -> new LoggedTunableNumber("Elevator/BargeSP", 4.0).getAsDouble(), ProfileType.MM_POSITION), + CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); private final DoubleSupplier output; - private final double feedFwd; private final ProfileType profileType; } diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java index c72adc2..1f310f7 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java @@ -27,8 +27,6 @@ public enum ProfileType { public interface TargetState { public DoubleSupplier getOutput(); - public double getFeedFwd(); - public ProfileType getProfileType(); } @@ -134,19 +132,19 @@ public void periodic() default: case POSITION: /* Run Closed Loop to position in rotations */ - io.runToPosition(getState().getOutput().getAsDouble(), getState().getFeedFwd()); + io.runToPosition(getState().getOutput().getAsDouble()); break; case VELOCITY: /* Run Closed Loop to velocity in rotations/second */ - io.runToVelocity(getState().getOutput().getAsDouble(), getState().getFeedFwd()); + io.runToVelocity(getState().getOutput().getAsDouble()); break; case MM_POSITION: /* Run Motion Magic to the specified position setpoint (in rotations) */ - io.runMotionMagicPosition(getState().getOutput().getAsDouble(), getState().getFeedFwd()); + io.runMotionMagicPosition(getState().getOutput().getAsDouble()); break; case MM_VELOCITY: /* Run Motion Magic to the specified velocity setpoint (in rotations/second) */ - io.runMotionMagicVelocity(getState().getOutput().getAsDouble(), getState().getFeedFwd()); + io.runMotionMagicVelocity(getState().getOutput().getAsDouble()); break; case OPEN_VOLTAGE: /* Run Open Loop using specified voltage in volts */ diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java index 1e7b6cd..56ab35f 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java @@ -38,19 +38,19 @@ public default void runCurrent(double amps) {} /** Run Closed Loop to position in rotations */ - public default void runToPosition(double position, double feedforward) + public default void runToPosition(double position) {} /** Run Closed Loop to velocity in rotations/second */ - public default void runToVelocity(double velocity, double feedforward) + public default void runToVelocity(double velocity) {} /** Run Motion Magic to the specified setpoint */ - public default void runMotionMagicPosition(double setpoint, double feedFwd) + public default void runMotionMagicPosition(double setpoint) {} /** Run Motion Magic to the specified velocity */ - public default void runMotionMagicVelocity(double velocity, double feedFwd) + public default void runMotionMagicVelocity(double velocity) {} /* Stop in Open Loop */ diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java index dad5ad3..93a2cba 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java @@ -350,35 +350,35 @@ public void runCurrent(double amps) /** Run Closed Loop to setpoint in rotations */ @Override - public void runToPosition(double position, double feedFwd) + public void runToPosition(double position) { - mMainMotor.setControl(positionControl.withPosition(position).withFeedForward(feedFwd)); + mMainMotor.setControl(positionControl.withPosition(position)); mOpSetpoint = position; } /** Run Closed Loop to velocity in rotations/second */ @Override - public void runToVelocity(double velocity, double feedFwd) + public void runToVelocity(double velocity) { - mMainMotor.setControl(velocityControl.withVelocity(velocity).withFeedForward(feedFwd)); + mMainMotor.setControl(velocityControl.withVelocity(velocity)); mOpSetpoint = velocity; } /** Run Motion Magic to the specified setpoint */ @Override - public void runMotionMagicPosition(double setpoint, double feedFwd) + public void runMotionMagicPosition(double setpoint) { mMainMotor.setControl( - motionMagicPositionControl.withPosition(setpoint).withFeedForward(feedFwd)); + motionMagicPositionControl.withPosition(setpoint)); mOpSetpoint = setpoint; } /** Run Motion Magic Velocity to the specified velocity */ @Override - public void runMotionMagicVelocity(double velocity, double feedFwd) + public void runMotionMagicVelocity(double velocity) { mMainMotor.setControl( - motionMagicVelocityControl.withVelocity(velocity).withFeedForward(feedFwd)); + motionMagicVelocityControl.withVelocity(velocity)); } /* Stop in Open Loop */ From 790d8ead54c22d7e893d7201bc98327f8e3553d2 Mon Sep 17 00:00:00 2001 From: Bryson Halsey Date: Sat, 15 Feb 2025 00:03:17 -0500 Subject: [PATCH 16/24] added alage bindings --- src/main/java/frc/robot/RobotContainer.java | 59 +++++++++------------ 1 file changed, 25 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62c9d08..62bc0f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,17 +75,12 @@ public class RobotContainer { private final Superstructure m_superStruct; public final Vision m_vision; - public final LED m_LED; + // public final LED m_LED; // Trigger for algae/coral mode switching - private boolean coralModeEnabled = false; + private boolean coralModeEnabled = true; private Trigger isCoralMode = new Trigger(() -> coralModeEnabled); - // private final LaserCANSensor m_clawLaserCAN = - // new LaserCANSensor(Ports.CLAW_LASERCAN.getDeviceNumber(), Inches.of(6)); - // private final LaserCANSensor m_rampLaserCAN = - // new LaserCANSensor(Ports.RAMP_LASERCAN.getDeviceNumber(), Inches.of(6)); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -182,8 +177,8 @@ public RobotContainer() m_superStruct = new Superstructure(m_profiledArm, m_profiledElevator); // LED subsystem reads status from all other subsystems to control LEDs via CANdle - m_LED = new LED(m_driver, m_profiledArm, m_clawRoller, m_profiledClimber, m_drive, - m_profiledElevator, m_vision, m_clawRollerLaserCAN, isCoralMode); + // m_LED = new LED(m_driver, m_profiledArm, m_clawRoller, m_profiledClimber, m_drive, + // m_profiledElevator, m_vision, m_clawRollerLaserCAN, isCoralMode); // Logic Triggers registerNamedCommands(); @@ -208,8 +203,6 @@ public RobotContainer() "Drive SysId (Dynamic Forward)", m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); m_autoChooser.addOption( "Drive SysId (Dynamic Reverse)", m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - m_autoChooser.addOption("Elevator static", m_profiledElevator.staticCharacterization(2.0)); - m_autoChooser.addOption("Arm static", m_profiledArm.staticCharacterization(2.0)); // Configure the controller button and joystick bindings configureControllerBindings(); @@ -252,23 +245,12 @@ public Command setCoralAlgaeModeCommand() }); } - /** Use this method to define your joystick and button -> command mappings. */ + /** Button and Command mappings */ private void configureControllerBindings() { // Default command, normal field-relative drive m_drive.setDefaultCommand(joystickDrive()); - // Driver Back Button: Reset gyro / odometry - final Runnable setPose = - Constants.currentMode == Constants.Mode.SIM - ? () -> m_drive.setPose(m_driveSimulation.getSimulatedDriveTrainPose()) - : () -> m_drive - .setPose(new Pose2d(m_drive.getPose().getTranslation(), new Rotation2d())); - m_driver - .back() - .onTrue( - Commands.runOnce(setPose).ignoringDisable(true)); - // Driver Left Bumper: Face Nearest Reef Face m_driver.leftBumper() .whileTrue( @@ -301,23 +283,35 @@ private void configureControllerBindings() // Driver A Button held and Right Bumper Pressed: Send Arm and Elevator to Processor m_driver - .a().and(isCoralMode.negate()) + .a().and(isCoralMode) .onTrue( m_superStruct.getTransitionCommand(Arm.State.ALGAE_GROUND, Elevator.State.ALGAE_SCORE)); // Driver X Button: Send Arm and Elevator to LEVEL_2 m_driver - .x() + .x().and(isCoralMode) .onTrue( m_superStruct.getTransitionCommand(Arm.State.LEVEL_2, Elevator.State.LEVEL_2)); + // Driver X Button: Send Arm and Elevator to LEVEL_2 + m_driver + .x().and(isCoralMode.negate()) + .onTrue( + m_superStruct.getTransitionCommand(Arm.State.ALGAE_LOW, Elevator.State.ALGAE_LOW)); + // Driver B Button: Send Arm and Elevator to LEVEL_3 m_driver - .b() + .b().and(isCoralMode) .onTrue( m_superStruct.getTransitionCommand(Arm.State.LEVEL_3, Elevator.State.LEVEL_3)); + m_driver + .b().and(isCoralMode.negate()) + .onTrue( + m_superStruct.getTransitionCommand(Arm.State.ALGAE_HIGH, + Elevator.State.ALGAE_HIGH)); + // Driver Y Button: Send Arm and Elevator to LEVEL_4 m_driver .y().and(isCoralMode) @@ -329,12 +323,11 @@ private void configureControllerBindings() m_driver .y().and(isCoralMode.negate()) .onTrue( - m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.BARGE)); - // TODO: Test Arm Level 4 in Sim + m_superStruct.getTransitionCommand(Arm.State.BARGE, Elevator.State.BARGE)); // Driver Right Trigger: Place Coral or Algae (Should be done once the robot is in position) m_driver.rightTrigger() - .whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.EJECT)); + .whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.SCORE)); // Driver Left Trigger: Drivetrain drive at coral station angle, prepare the elevator and // arm, Get Ready to Intake Coral @@ -357,6 +350,7 @@ private void configureControllerBindings() m_clawRoller.setStateCommand(ClawRoller.State.OFF), m_profiledArm.setStateCommand(Arm.State.STOW)))); + // Driver Left Trigger + Right Bumper: Algae Intake m_driver.leftTrigger().and(isCoralMode.negate()).whileTrue( Commands.sequence( @@ -422,11 +416,8 @@ private void configureControllerBindings() // Driver Right Bumper: Toggle between Coral and Algae Modes. // Make sure the Approach nearest reef face does not mess with this - m_driver.rightBumper() - .onTrue( - !m_driver.leftBumper().getAsBoolean() ? setCoralAlgaeModeCommand() - : Commands.runOnce(() -> { - })); + m_driver.rightBumper().and(m_driver.leftBumper().negate()) + .onTrue(setCoralAlgaeModeCommand()); } From 3215cf4ac9a0e3e23fa324ed46df6d492aaaf1d0 Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Sat, 15 Feb 2025 10:32:36 -0500 Subject: [PATCH 17/24] Add arm/elev setpoint tuning --- .../java/frc/robot/subsystems/Arm/Arm.java | 28 +++++++++-------- .../Claw/ClawRoller/ClawRoller.java | 5 +++- .../frc/robot/subsystems/Climber/Climber.java | 10 +++++-- .../robot/subsystems/Elevator/Elevator.java | 30 +++++++++++-------- 4 files changed, 43 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index 835f28a..3b45d24 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -17,23 +17,25 @@ @Getter public class Arm extends GenericMotionProfiledSubsystem { + static LoggedTunableNumber positionTuning = new LoggedTunableNumber("Arm/PositionTuningSP", 124.0); + @RequiredArgsConstructor @Getter public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), - STOW(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/StowSP", 124.0).getAsDouble()), ProfileType.MM_POSITION), - - CORAL_INTAKE(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/CoralIntakeSP", 144.0).getAsDouble()), ProfileType.MM_POSITION), - LEVEL_1(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L1SP", 120.0).getAsDouble()), ProfileType.MM_POSITION), - LEVEL_2(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L2SP", 105.0).getAsDouble()), ProfileType.MM_POSITION), - LEVEL_3(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L3SP", 105.0).getAsDouble()), ProfileType.MM_POSITION), - LEVEL_4(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/L4SP", 90.0).getAsDouble()), ProfileType.MM_POSITION), - CLIMB(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/ClimbSP", 50.4).getAsDouble()), ProfileType.MM_POSITION), - ALGAE_LOW(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeLowSP", 114.0).getAsDouble()), ProfileType.MM_POSITION), - ALGAE_HIGH(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeHighSP", 114.0).getAsDouble()), ProfileType.MM_POSITION), - ALGAE_GROUND(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeGroundSP", 9.0).getAsDouble()), ProfileType.MM_POSITION), - ALGAE_SCORE(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/AlgaeScoreSP", 114.0).getAsDouble()), ProfileType.MM_POSITION), - BARGE(() -> Units.degreesToRotations(new LoggedTunableNumber("Arm/BargeSP", 30.0).getAsDouble()), ProfileType.MM_POSITION), + STOW(() -> Units.degreesToRotations(124.0), ProfileType.MM_POSITION), + CORAL_INTAKE(() -> Units.degreesToRotations(144.0), ProfileType.MM_POSITION), + LEVEL_1(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION), + LEVEL_2(() -> Units.degreesToRotations(105.0), ProfileType.MM_POSITION), + LEVEL_3(() -> Units.degreesToRotations(105.0), ProfileType.MM_POSITION), + LEVEL_4(() -> Units.degreesToRotations(90.0), ProfileType.MM_POSITION), + CLIMB(() -> Units.degreesToRotations(50.4), ProfileType.MM_POSITION), + ALGAE_LOW(() -> Units.degreesToRotations(114.0), ProfileType.MM_POSITION), + ALGAE_HIGH(() -> Units.degreesToRotations(114.0), ProfileType.MM_POSITION), + ALGAE_GROUND(() -> Units.degreesToRotations(9.0), ProfileType.MM_POSITION), + ALGAE_SCORE(() -> Units.degreesToRotations(114.0), ProfileType.MM_POSITION), + BARGE(() -> Units.degreesToRotations(30.0), ProfileType.MM_POSITION), + TUNING(() -> Units.degreesToRotations(positionTuning.getAsDouble()), ProfileType.MM_POSITION), CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); private final DoubleSupplier output; diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 3c2a170..9ff04e9 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -20,6 +20,8 @@ public class ClawRoller // real // numbers + static LoggedTunableNumber holdCoralSP = new LoggedTunableNumber("ClawRoller/HoldCoralSP", 0.0); + @RequiredArgsConstructor @Getter public enum State implements TargetState { @@ -27,7 +29,8 @@ public enum State implements TargetState { INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE), EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(() -> new LoggedTunableNumber("ClawRoller/HoldCoralSP", 0.0).getAsDouble(), ProfileType.MM_POSITION); // Wheels should spin x rotations before stopping + HOLDCORAL(() -> 0.6, ProfileType.MM_POSITION), // Wheels should spin x rotations before stopping + TUNING(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); private final DoubleSupplier output; private final ProfileType profileType; diff --git a/src/main/java/frc/robot/subsystems/Climber/Climber.java b/src/main/java/frc/robot/subsystems/Climber/Climber.java index c33aea8..d814492 100644 --- a/src/main/java/frc/robot/subsystems/Climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber/Climber.java @@ -23,13 +23,17 @@ @Getter public class Climber extends GenericMotionProfiledSubsystem { + static LoggedTunableNumber positionTuning = new LoggedTunableNumber("Climber/PositionTuningSP", 0.0); + @RequiredArgsConstructor @Getter public enum State implements TargetState { // HOME is climber upright, Prep - Assuming that PREP position is parallel to the x axis, CLIMB is inwards - HOME(() -> Units.degreesToRotations(new LoggedTunableNumber("Climber/HomeSP", 90.0).getAsDouble()), ProfileType.MM_POSITION), - PREP(() -> Units.degreesToRotations(new LoggedTunableNumber("Climber/PrepSP", 0.0).getAsDouble()), ProfileType.MM_POSITION), - CLIMB(() -> Units.degreesToRotations(new LoggedTunableNumber("Climber/ClimbSP", 110.0).getAsDouble()), ProfileType.MM_POSITION); + HOME(() -> Units.degreesToRotations(90.0), ProfileType.MM_POSITION), + PREP(() -> Units.degreesToRotations(0.0), ProfileType.MM_POSITION), + CLIMB(() -> Units.degreesToRotations(110.0), ProfileType.MM_POSITION), + TUNING(() -> Units.degreesToRotations(positionTuning.getAsDouble()), ProfileType.MM_POSITION), + ; private final DoubleSupplier output; private final ProfileType profileType; diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java index bf1012e..9af8ee1 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java @@ -22,23 +22,27 @@ @Getter public class Elevator extends GenericMotionProfiledSubsystem { + static LoggedTunableNumber homingTuning = new LoggedTunableNumber("Elevator/HomingVoltageSP", -0.2); + static LoggedTunableNumber positionTuning = new LoggedTunableNumber("Elevator/PositionTuningSP", 0.05); + @RequiredArgsConstructor @Getter public enum State implements TargetState { - HOMING(() -> new LoggedTunableNumber("Elevator/HomingVoltageSP", -0.2).getAsDouble(), ProfileType.OPEN_VOLTAGE), + HOMING(() -> homingTuning.getAsDouble(), ProfileType.OPEN_VOLTAGE), // TODO: Test Voltage and position values (rotations) - STOW(() -> new LoggedTunableNumber("Elevator/StowSP", 0.15).getAsDouble(), ProfileType.MM_POSITION), - CORAL_INTAKE(() -> new LoggedTunableNumber("Elevator/CoralIntakeSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), - LEVEL_1(() -> new LoggedTunableNumber("Elevator/L1SP", 0.2).getAsDouble(), ProfileType.MM_POSITION), - LEVEL_2(() -> new LoggedTunableNumber("Elevator/L2SP", 1.4).getAsDouble(), ProfileType.MM_POSITION), - LEVEL_3(() -> new LoggedTunableNumber("Elevator/L3SP", 2.85).getAsDouble(), ProfileType.MM_POSITION), - LEVEL_4(() -> new LoggedTunableNumber("Elevator/L4SP", 5.2).getAsDouble(), ProfileType.MM_POSITION), - CLIMB(() -> new LoggedTunableNumber("Elevator/ClimbSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), - ALGAE_LOW(() -> new LoggedTunableNumber("Elevator/AlgaeLowSP", 0.8).getAsDouble(), ProfileType.MM_POSITION), - ALGAE_HIGH(() -> new LoggedTunableNumber("Elevator/AlgaeHighSP", 1.5).getAsDouble(), ProfileType.MM_POSITION), - ALGAE_GROUND(() -> new LoggedTunableNumber("Elevator/AlgaeGroundSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), - ALGAE_SCORE(() -> new LoggedTunableNumber("Elevator/AlgaeScoreSP", 0.05).getAsDouble(), ProfileType.MM_POSITION), - BARGE(() -> new LoggedTunableNumber("Elevator/BargeSP", 4.0).getAsDouble(), ProfileType.MM_POSITION), + STOW(() -> 0.15, ProfileType.MM_POSITION), + CORAL_INTAKE(() -> 0.05, ProfileType.MM_POSITION), + LEVEL_1(() -> 0.2, ProfileType.MM_POSITION), + LEVEL_2(() -> 1.4, ProfileType.MM_POSITION), + LEVEL_3(() -> 2.85, ProfileType.MM_POSITION), + LEVEL_4(() -> 5.20, ProfileType.MM_POSITION), + CLIMB(() -> 0.05, ProfileType.MM_POSITION), + ALGAE_LOW(() -> 0.8, ProfileType.MM_POSITION), + ALGAE_HIGH(() -> 1.5, ProfileType.MM_POSITION), + ALGAE_GROUND(() -> 0.05, ProfileType.MM_POSITION), + ALGAE_SCORE(() -> 0.05, ProfileType.MM_POSITION), + BARGE(() -> 4.0, ProfileType.MM_POSITION), + TUNING(() -> positionTuning.getAsDouble(), ProfileType.MM_POSITION), CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); private final DoubleSupplier output; From b11e6009d7e62da331140ff87c127aae296f26b7 Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Sat, 15 Feb 2025 11:06:24 -0500 Subject: [PATCH 18/24] merge arm --- .../java/frc/robot/subsystems/Arm/Arm.java | 30 ++++--------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index cfd9801..2f5617f 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -17,35 +17,15 @@ @Getter public class Arm extends GenericMotionProfiledSubsystem { -<<<<<<< HEAD + static LoggedTunableNumber positionTuning = + new LoggedTunableNumber("Arm/PositionTuningSP", 124.0); + // .14 rot is the max extension -======= - static LoggedTunableNumber positionTuning = new LoggedTunableNumber("Arm/PositionTuningSP", 124.0); ->>>>>>> main @RequiredArgsConstructor @Getter public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), -<<<<<<< HEAD - STOW(Units.degreesToRotations(124.0), 0.0, ProfileType.MM_POSITION), - - CORAL_INTAKE(Units.degreesToRotations(144.0), 0.0, ProfileType.MM_POSITION), - LEVEL_1(Units.degreesToRotations(120.0), 0.0, ProfileType.MM_POSITION), - LEVEL_2(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION), - LEVEL_3(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION), - LEVEL_4(Units.degreesToRotations(90.0), 0.0, ProfileType.MM_POSITION), - CLIMB(.14, 0.0, ProfileType.MM_POSITION), - ALGAE_LOW(Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), - ALGAE_HIGH(Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), - ALGAE_GROUND(Units.degreesToRotations(9.0), 0.0, ProfileType.MM_POSITION), - ALGAE_SCORE(Units.degreesToRotations(114.0), 0.0, ProfileType.MM_POSITION), - BARGE(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION), - CHARACTERIZATION(0.0, 0.0, ProfileType.CHARACTERIZATION); - - private final double output; - private final double feedFwd; -======= STOW(() -> Units.degreesToRotations(124.0), ProfileType.MM_POSITION), CORAL_INTAKE(() -> Units.degreesToRotations(144.0), ProfileType.MM_POSITION), LEVEL_1(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION), @@ -58,9 +38,9 @@ public enum State implements TargetState { ALGAE_GROUND(() -> Units.degreesToRotations(9.0), ProfileType.MM_POSITION), ALGAE_SCORE(() -> Units.degreesToRotations(114.0), ProfileType.MM_POSITION), BARGE(() -> Units.degreesToRotations(30.0), ProfileType.MM_POSITION), - TUNING(() -> Units.degreesToRotations(positionTuning.getAsDouble()), ProfileType.MM_POSITION), + TUNING(() -> Units.degreesToRotations(positionTuning.getAsDouble()), + ProfileType.MM_POSITION), CHARACTERIZATION(() -> 0.0, ProfileType.CHARACTERIZATION); ->>>>>>> main private final DoubleSupplier output; private final ProfileType profileType; From 281b7daea12354ebc0d867ca2c24721c1195d8e6 Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Sat, 15 Feb 2025 11:07:42 -0500 Subject: [PATCH 19/24] set roller tuning to hold coral --- .../frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 8eec012..73cc431 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -16,9 +16,7 @@ public class ClawRoller extends GenericMotionProfiledSubsystem { - public final Trigger stalled = new Trigger(() -> super.inputs.supplyCurrentAmps[0] < 1); // TODO: - // real - // numbers + public final Trigger stalled = new Trigger(() -> super.inputs.torqueCurrentAmps[0] >= 30); static LoggedTunableNumber holdCoralSP = new LoggedTunableNumber("ClawRoller/HoldCoralSP", 0.0); @@ -29,9 +27,7 @@ public enum State implements TargetState { INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE), EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(() -> 0.6, ProfileType.MM_POSITION), // Wheels should spin x rotations before - // stopping - TUNING(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); + HOLDCORAL(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); private final DoubleSupplier output; private final ProfileType profileType; From 52abbe3b4ff09cd317258dd6aba7411343d15e0f Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Sat, 15 Feb 2025 11:08:26 -0500 Subject: [PATCH 20/24] Intake coral logic? --- src/main/java/frc/robot/RobotContainer.java | 23 +++++++++++++------ .../frc/robot/subsystems/Superstructure.java | 14 ----------- 2 files changed, 16 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fcb5e0a..2f0bd51 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.subsystems.Climber.ClimberIOSim; import frc.robot.subsystems.Climber.ClimberIOTalonFX; import frc.robot.subsystems.Elevator.*; +import frc.robot.subsystems.Elevator.Elevator.State; import frc.robot.subsystems.LED.LED; import frc.robot.subsystems.Vision.*; import frc.robot.subsystems.drive.*; @@ -319,7 +320,7 @@ private void configureControllerBindings() m_driver .a() .onTrue( - m_superStruct.getTransitionCommand(Arm.State.LEVEL_1, Elevator.State.LEVEL_1)); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_1, Elevator.State.LEVEL_1, 0.2, 0.2)); // Driver A Button held and Right Bumper Pressed: Send Arm and Elevator to Processor // m_driver @@ -332,19 +333,19 @@ private void configureControllerBindings() m_driver .x() .onTrue( - m_superStruct.getTransitionCommand(Arm.State.LEVEL_2, Elevator.State.LEVEL_2)); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_2, Elevator.State.LEVEL_2, 0.2, 0.2)); // Driver B Button: Send Arm and Elevator to LEVEL_3 m_driver .b() .onTrue( - m_superStruct.getTransitionCommand(Arm.State.LEVEL_3, Elevator.State.LEVEL_3)); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_3, Elevator.State.LEVEL_3, 0.2, 0.2)); // Driver Y Button: Send Arm and Elevator to LEVEL_4 m_driver .y() .onTrue( - m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4)); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4, 0.2, 0.2)); // Driver Y Button held and Right Bumper having been pressed to ALGAE mode: Send Arm and // Elevator to NET @@ -366,12 +367,20 @@ private void configureControllerBindings() .leftTrigger() .whileTrue( Commands.sequence( - m_profiledElevator.setStateCommand(Elevator.State.CORAL_INTAKE), + // Always move Arm to STOW position before moving Elevator + m_profiledArm.setStateCommand(Arm.State.STOW).until(() -> m_profiledArm.atPosition(0.1)), + // Move Elevator to new position + Commands.waitUntil(() -> m_profiledArm.atPosition(0.1)) + .andThen(m_profiledElevator.setStateCommand(State.CORAL_INTAKE) + .until(() -> m_profiledElevator.atPosition(0.1))), + // Once Elevator in position, move Arm to Coral Intake and start spinning claw Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) .andThen(Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.CORAL_INTAKE), + m_profiledArm.setStateCommand(Arm.State.CORAL_INTAKE).until(() -> m_profiledArm.atPosition(0.1)), m_clawRoller.setStateCommand(ClawRoller.State.INTAKE)) - .until(m_clawRollerLaserCAN.triggered)))); + .until(m_clawRollerLaserCAN.triggered)), + m_clawRoller.setStateCommand(ClawRoller.State.HOLDCORAL))); + // m_driver // .leftTrigger().and(isCoralMode) // .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index e50882d..0e430f3 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -50,18 +50,4 @@ public Command getTransitionCommand(Arm.State armState, Elevator.State elevatorS Commands.waitUntil(() -> m_Elevator.atPosition(elevTolerance)).andThen( m_Arm.setStateCommand(armState).until(() -> m_Arm.atPosition(armTolerance))))); } - - /** - * Get a Command to transition the states of the Arm and Elevator in the proper order. - * - * This version uses the defualt tolerances as specified by the subsystem's kminTolerance - * - * @param armState - * @param elevatorState - * @return A Command - */ - public Command getTransitionCommand(Arm.State armState, Elevator.State elevatorState) - { - return getTransitionCommand(armState, elevatorState, 0.0, 0.0); - } } From 323e923b770df4d9337005acbf21577b016bcb60 Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Sat, 15 Feb 2025 14:43:49 -0500 Subject: [PATCH 21/24] 2/15 --- src/main/java/frc/robot/RobotContainer.java | 38 ++++++++++++------- .../frc/robot/generated/TunerConstants.java | 2 +- .../java/frc/robot/subsystems/Arm/Arm.java | 4 +- .../Claw/ClawRoller/ClawRoller.java | 2 + .../Claw/ClawRoller/ClawRollerConstants.java | 4 +- .../ClawRollerLaserCANConstants.java | 2 +- .../Claw/IntakeLaserCAN/IntakeLaserCAN.java | 2 +- .../IntakeLaserCANConstants.java | 4 +- .../robot/subsystems/Elevator/Elevator.java | 6 +-- .../GenericLaserCANSubsystem/LaserCANSim.java | 2 +- 10 files changed, 40 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd87b91..16c6f0d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,10 +102,10 @@ public RobotContainer() // (robotPose) -> { // }); - // m_profiledArm = new Arm(new ArmIOTalonFX(), false); - // m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); + m_profiledArm = new Arm(new ArmIOTalonFX(), false); + m_profiledElevator = new Elevator(new ElevatorIOTalonFX(), false); // m_profiledClimber = new Climber(new ClimberIOTalonFX(), false); - // m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); + m_clawRoller = new ClawRoller(new ClawRollerIOTalonFX(), false); m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIOReal()); m_intakeLaserCAN = new IntakeLaserCAN(new IntakeLaserCANIOReal()); @@ -127,10 +127,10 @@ public RobotContainer() (robotPose) -> { }); - m_profiledArm = new Arm(new ArmIO() {}, true); - m_profiledElevator = new Elevator(new ElevatorIO() {}, true); + // m_profiledArm = new Arm(new ArmIO() {}, true); + // m_profiledElevator = new Elevator(new ElevatorIO() {}, true); m_profiledClimber = new Climber(new ClimberIO() {}, true); - m_clawRoller = new ClawRoller(new ClawRollerIO() {}, true); + // m_clawRoller = new ClawRoller(new ClawRollerIO() {}, true); // m_clawRollerLaserCAN = new ClawRollerLaserCAN(new ClawRollerLaserCANIO() {}); // m_intakeLaserCAN = new IntakeLaserCAN(new IntakeLaserCANIO() {}); @@ -365,13 +365,25 @@ private void configureControllerBindings() m_driver .leftTrigger() .whileTrue( - Commands.sequence( - m_profiledElevator.setStateCommand(Elevator.State.CORAL_INTAKE), - Commands.waitUntil(() -> m_profiledElevator.atPosition(0.1)) - .andThen(Commands.parallel( - m_profiledArm.setStateCommand(Arm.State.CORAL_INTAKE), - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE)) - .until(m_clawRollerLaserCAN.triggered)))); + m_clawRoller.setStateCommandNoEnd(ClawRoller.State.INTAKE) + .andThen( + m_superStruct + .getTransitionCommand(Arm.State.CORAL_INTAKE, + Elevator.State.CORAL_INTAKE)) + .andThen(Commands.waitUntil(m_intakeLaserCAN.triggered)) + .andThen(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.INTAKESLOW)) + .andThen(Commands + .waitUntil(m_intakeLaserCAN.triggered.negate() + .and(m_clawRollerLaserCAN.triggered))) + .andThen(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.SHUFFLE)) + .andThen(Commands + .waitUntil(m_intakeLaserCAN.triggered + .and(m_clawRollerLaserCAN.triggered))) + .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) + .onFalse(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.OFF).andThen(m_superStruct + .getTransitionCommand(Arm.State.STOW, + Elevator.State.STOW))); + // m_driver // .leftTrigger().and(isCoralMode) // .whileTrue( diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index a2b8366..eff43c1 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -97,7 +97,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 8; + private static final int kPigeonId = 1; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java index 2f5617f..9d46f60 100644 --- a/src/main/java/frc/robot/subsystems/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm/Arm.java @@ -27,7 +27,7 @@ public class Arm extends GenericMotionProfiledSubsystem { public enum State implements TargetState { // HOMING(0.0, 0.0, ProfileType.MM_POSITION), STOW(() -> Units.degreesToRotations(124.0), ProfileType.MM_POSITION), - CORAL_INTAKE(() -> Units.degreesToRotations(144.0), ProfileType.MM_POSITION), + CORAL_INTAKE(() -> 0.42, ProfileType.MM_POSITION), LEVEL_1(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION), LEVEL_2(() -> Units.degreesToRotations(105.0), ProfileType.MM_POSITION), LEVEL_3(() -> Units.degreesToRotations(105.0), ProfileType.MM_POSITION), @@ -64,7 +64,7 @@ public Arm(ArmIO io, boolean isSim) /** Constructor */ public Command setStateCommand(State state) { - return runOnce(() -> this.state = state); + return this.runOnce(() -> this.state = state); } public boolean atPosition(double tolerance) diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 73cc431..2ef8f02 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -25,6 +25,8 @@ public class ClawRoller public enum State implements TargetState { OFF(() -> 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE), + INTAKESLOW(() -> 1, ProfileType.OPEN_VOLTAGE), + SHUFFLE(() -> -0.5, ProfileType.OPEN_VOLTAGE), EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), HOLDCORAL(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java index a6fa950..8038a11 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java @@ -41,11 +41,11 @@ public final class ClawRollerConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 1.0; // TODO: tune on real robot + kSubSysConstants.kMotorConfig.Slot0.kP = 5; // TODO: tune on real robot kSubSysConstants.kMotorConfig.Slot0.kI = 0; kSubSysConstants.kMotorConfig.Slot0.kD = 0; kSubSysConstants.kMotorConfig.Slot0.kG = 0; - kSubSysConstants.kMotorConfig.Slot0.kS = 36; + kSubSysConstants.kMotorConfig.Slot0.kS = 0; kSubSysConstants.kMotorConfig.Slot0.kV = 0; kSubSysConstants.kMotorConfig.Slot0.kA = 0; kSubSysConstants.kMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 100; diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCANConstants.java b/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCANConstants.java index 700b38b..c292b04 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCANConstants.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRollerLaserCAN/ClawRollerLaserCANConstants.java @@ -14,7 +14,7 @@ public class ClawRollerLaserCANConstants { kSubSysConstants.kName = "ClawRollerLaserCAN"; kSubSysConstants.laserCANDeviceId = Ports.CLAW_LASERCAN; kSubSysConstants.rangingMode = RangingMode.SHORT; - kSubSysConstants.regionOfInterest = new RegionOfInterest(10, 10, 10, 10); + kSubSysConstants.regionOfInterest = new RegionOfInterest(6, 6, 8, 8); kSubSysConstants.timingBudget = TimingBudget.TIMING_BUDGET_20MS; } } diff --git a/src/main/java/frc/robot/subsystems/Claw/IntakeLaserCAN/IntakeLaserCAN.java b/src/main/java/frc/robot/subsystems/Claw/IntakeLaserCAN/IntakeLaserCAN.java index 2113848..c0ac65c 100644 --- a/src/main/java/frc/robot/subsystems/Claw/IntakeLaserCAN/IntakeLaserCAN.java +++ b/src/main/java/frc/robot/subsystems/Claw/IntakeLaserCAN/IntakeLaserCAN.java @@ -16,7 +16,7 @@ public class IntakeLaserCAN extends GenericLaserCANSubsystem { public enum State implements TargetState { HOMING(() -> homingTuning.getAsDouble(), ProfileType.OPEN_VOLTAGE), // TODO: Test Voltage and position values (rotations) - STOW(() -> 0.15, ProfileType.MM_POSITION), - CORAL_INTAKE(() -> 0.05, ProfileType.MM_POSITION), + STOW(() -> 0, ProfileType.MM_POSITION), + CORAL_INTAKE(() -> 0, ProfileType.MM_POSITION), LEVEL_1(() -> 0.2, ProfileType.MM_POSITION), LEVEL_2(() -> 1.4, ProfileType.MM_POSITION), LEVEL_3(() -> 2.85, ProfileType.MM_POSITION), @@ -70,7 +70,7 @@ public Elevator(ElevatorIO io, boolean isSim) public Command setStateCommand(State state) { - return runOnce(() -> this.state = state); + return this.runOnce(() -> this.state = state); } private Debouncer homedDebouncer = new Debouncer(.25, DebounceType.kRising); diff --git a/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/LaserCANSim.java b/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/LaserCANSim.java index c99f905..c38a3eb 100644 --- a/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/LaserCANSim.java +++ b/src/main/java/frc/robot/subsystems/GenericLaserCANSubsystem/LaserCANSim.java @@ -14,7 +14,7 @@ public class LaserCANSim implements LaserCanInterface { public LaserCANSim(String name) { - tunableDistance = new LoggedTunableNumber(name + "/Measurement", 0); + tunableDistance = new LoggedTunableNumber(name + "/Measurement", Double.POSITIVE_INFINITY); } @Override From 9d6d134d7b9b57bde050ed9369fba424a2bbb84a Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Sat, 15 Feb 2025 14:50:37 -0500 Subject: [PATCH 22/24] merge --- src/main/java/frc/robot/RobotContainer.java | 74 ++++++------------- .../Claw/ClawRoller/ClawRoller.java | 5 +- 2 files changed, 26 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5851948..52812e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -375,17 +375,10 @@ private void configureControllerBindings() // Driver Left Trigger + Right Bumper: Algae Intake - // m_driver.leftTrigger().and(isCoralMode.negate()).whileTrue( - // Commands.sequence( - // (FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT) - // .getTranslation().getX() > 0) - // ? m_profiledElevator.setStateCommand(Elevator.State.ALGAE_HIGH) - // : m_profiledElevator.setStateCommand(Elevator.State.ALGAE_LOW), - // m_clawRoller.setStateCommand(ClawRoller.State.INTAKE), - // Commands.waitUntil(m_clawRoller.stalled) - // .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) - // .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))); - // m_driver.rightTrigger().whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.SCORE)); + m_driver.leftTrigger().and(isCoralMode.negate()).onTrue( + (m_clawRoller.getState() == ClawRoller.State.ALGAE_INTAKE) + ? m_clawRoller.setStateCommandNoEnd(ClawRoller.State.EJECT) + : m_clawRoller.setStateCommandNoEnd(ClawRoller.State.ALGAE_INTAKE)); // Driver Start Button: Climb Request (toggle) // m_driver.start().onTrue(Commands.runOnce(() -> { @@ -433,9 +426,14 @@ private void configureControllerBindings() // Driver POV Down: Zero the Elevator (HOMING) m_driver.povDown().whileTrue( - m_profiledElevator.setStateCommand(Elevator.State.HOMING) - .until(m_profiledElevator.getHomedTrigger()) - .andThen(m_profiledElevator.zeroSensorCommand())); + Commands.sequence( + // Always move Arm to STOW position before moving Elevator + m_profiledArm.setStateCommand(Arm.State.STOW).until(() -> m_profiledArm.atPosition(0.1)), + // Move Elevator to homing position + Commands.waitUntil(() -> m_profiledArm.atPosition(0.1)) + .andThen(m_profiledElevator.setStateCommand(Elevator.State.HOMING) + .until(m_profiledElevator.getHomedTrigger())) + .andThen(m_profiledElevator.zeroSensorCommand()))); // Driver Right Bumper: Toggle between Coral and Algae Modes. // Make sure the Approach nearest reef face does not mess with this @@ -452,38 +450,23 @@ private void registerNamedCommands() // Go to the L1 Position NamedCommands.registerCommand( "L1", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_1), - m_profiledArm.setStateCommand(Arm.State.LEVEL_1))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_1, Elevator.State.LEVEL_1)); // Go to the L2 Position NamedCommands.registerCommand( "L2", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_2), - m_profiledArm.setStateCommand(Arm.State.LEVEL_2))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_2, Elevator.State.LEVEL_2)); // Go to the L3 Position NamedCommands.registerCommand( "L3", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_3), - m_profiledArm.setStateCommand(Arm.State.LEVEL_3))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_3, Elevator.State.LEVEL_3)); // Go to the L4 Position NamedCommands.registerCommand( "L4", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_4), - Commands.waitUntil(() -> m_profiledElevator.atPosition(1)) - .andThen(m_profiledArm.setStateCommand(Arm.State.LEVEL_4)))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4)); // Go to the Home Position NamedCommands.registerCommand( "Home", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.STOW), - m_profiledArm.setStateCommand(Arm.State.STOW))); - - // Wait for intake laserCAN to be triggered - NamedCommands.registerCommand("WaitForCoral", - Commands.waitUntil(m_intakeLaserCAN.triggered)); + m_superStruct.getTransitionCommand(Arm.State.STOW, Elevator.State.STOW)); // Wait for intake laserCAN to be triggered NamedCommands.registerCommand("WaitForCoral", @@ -492,23 +475,12 @@ private void registerNamedCommands() // Intake Coral NamedCommands.registerCommand( "IntakeCoral", - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE) - .until(m_clawRollerLaserCAN.triggered) - .andThen(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.HOLDCORAL)) - .andThen(Commands.waitUntil(() -> m_clawRoller.atPosition(1)))); - - // Intake Algae - NamedCommands.registerCommand( - "IntakeAlgae", - Commands.sequence( - (FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT) - .getTranslation().getX() > 0) - ? m_profiledElevator.setStateCommand(Elevator.State.ALGAE_HIGH) - : m_profiledElevator.setStateCommand(Elevator.State.ALGAE_LOW), - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE), - Commands.waitUntil(m_clawRoller.stalled) - .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) - .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))); + Commands.parallel( + m_superStruct.getTransitionCommand(Arm.State.CORAL_INTAKE, Elevator.State.CORAL_INTAKE), + m_clawRoller.setStateCommand(ClawRoller.State.INTAKE) + .until(m_clawRollerLaserCAN.triggered) + .andThen(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.HOLDCORAL)) + .andThen(Commands.waitUntil(() -> m_clawRoller.atPosition(1))))); NamedCommands.registerCommand( "Score", diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 9ff04e9..d1361e0 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -21,6 +21,7 @@ public class ClawRoller // numbers static LoggedTunableNumber holdCoralSP = new LoggedTunableNumber("ClawRoller/HoldCoralSP", 0.0); + static LoggedTunableNumber algaeIntakeSP = new LoggedTunableNumber("ClawRoller/AlgaeIntakeSP", -15.0); @RequiredArgsConstructor @Getter @@ -29,8 +30,8 @@ public enum State implements TargetState { INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE), EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(() -> 0.6, ProfileType.MM_POSITION), // Wheels should spin x rotations before stopping - TUNING(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); + HOLDCORAL(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION), + ALGAE_INTAKE(() -> algaeIntakeSP.getAsDouble(), ProfileType.OPEN_CURRENT); private final DoubleSupplier output; private final ProfileType profileType; From e71124c90b81bb269712f284d9539c804c0bbe4c Mon Sep 17 00:00:00 2001 From: Driver3467 Date: Sat, 15 Feb 2025 15:09:15 -0500 Subject: [PATCH 23/24] Functional Intake --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- .../frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java | 2 +- .../subsystems/Claw/ClawRoller/ClawRollerConstants.java | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e3b55d..8a6e63f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -380,10 +380,11 @@ private void configureControllerBindings() .andThen(Commands .waitUntil(m_intakeLaserCAN.triggered .and(m_clawRollerLaserCAN.triggered))) - .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) - .onFalse(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.OFF).andThen(m_superStruct - .getTransitionCommand(Arm.State.STOW, - Elevator.State.STOW))); + .andThen(m_clawRoller.setStateCommand(ClawRoller.State.HOLDCORAL))) + .onFalse(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.HOLDCORAL) + .andThen(m_superStruct + .getTransitionCommand(Arm.State.STOW, + Elevator.State.STOW))); // m_driver // .leftTrigger().and(isCoralMode) diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 2ef8f02..1d77997 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -29,7 +29,7 @@ public enum State implements TargetState { SHUFFLE(() -> -0.5, ProfileType.OPEN_VOLTAGE), EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); + HOLDCORAL(() -> 0, ProfileType.MM_POSITION); private final DoubleSupplier output; private final ProfileType profileType; diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java index 8038a11..6fb40d6 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java @@ -41,7 +41,7 @@ public final class ClawRollerConstants { kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; /* REAL system profile constants */ - kSubSysConstants.kMotorConfig.Slot0.kP = 5; // TODO: tune on real robot + kSubSysConstants.kMotorConfig.Slot0.kP = 50; // TODO: tune on real robot kSubSysConstants.kMotorConfig.Slot0.kI = 0; kSubSysConstants.kMotorConfig.Slot0.kD = 0; kSubSysConstants.kMotorConfig.Slot0.kG = 0; From 4dc5b11e1e4620454947d023030bca0d66c8065a Mon Sep 17 00:00:00 2001 From: skukshtel Date: Sat, 15 Feb 2025 15:45:12 -0500 Subject: [PATCH 24/24] Add DISABLED_COAST and DISABLED_BRAKE as new ProfileTypes in GenericMotionMagicSubsystem --- .../GenericMotionProfiledSubsystem.java | 10 ++++++++++ .../GenericMotionProfiledSubsystemIO.java | 8 ++++++++ .../GenericMotionProfiledSubsystemIOImpl.java | 18 ++++++++++++++++++ 3 files changed, 36 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java index 1f310f7..28ce837 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java @@ -21,6 +21,8 @@ public enum ProfileType { MM_VELOCITY, OPEN_VOLTAGE, OPEN_CURRENT, + DISABLED_COAST, + DISABLED_BRAKE, CHARACTERIZATION } @@ -154,6 +156,14 @@ public void periodic() /* Run Open Loop using specified current in amps */ io.runCurrent(getState().getOutput().getAsDouble()); break; + case DISABLED_COAST: + /* Stop all output and put motor in Coast mode */ + io.stopCoast(); + break; + case DISABLED_BRAKE: + /* Stop all output and put motor in Brake mode */ + io.stopBrake(); + break; case CHARACTERIZATION: /* * Run Open Loop for characterization in the child subsystem class's diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java index 56ab35f..5e87be2 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIO.java @@ -53,6 +53,14 @@ public default void runMotionMagicPosition(double setpoint) public default void runMotionMagicVelocity(double velocity) {} + /* Stop in Coast mode */ + public default void stopCoast() + {} + + /* Stop in Brake mode */ + public default void stopBrake() + {} + /* Stop in Open Loop */ public default void stop() {} diff --git a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java index 93a2cba..aaa9c94 100644 --- a/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java +++ b/src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystemIOImpl.java @@ -3,10 +3,12 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; @@ -87,6 +89,8 @@ public class GenericMotionProfiledSubsystemIOImpl implements GenericMotionProfil new MotionMagicTorqueCurrentFOC(0.0).withUpdateFreqHz(0.0).withSlot(0); private final MotionMagicVelocityTorqueCurrentFOC motionMagicVelocityControl = new MotionMagicVelocityTorqueCurrentFOC(0.0).withUpdateFreqHz(0.0).withSlot(0); + private final CoastOut coastOut = new CoastOut(); + private final StaticBrake staticBrake = new StaticBrake(); /* * Constructor @@ -381,6 +385,20 @@ public void runMotionMagicVelocity(double velocity) motionMagicVelocityControl.withVelocity(velocity)); } + /* Stop in Coast mode */ + @Override + public void stopCoast() + { + mMainMotor.setControl(coastOut); + } + + /* Stop in Brake mode */ + @Override + public void stopBrake() + { + mMainMotor.setControl(staticBrake); + } + /* Stop in Open Loop */ @Override public void stop()