diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af2f8c8..f5d5be3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.CMD_DriveAlign; @@ -84,8 +85,18 @@ private void configureButtonBindings() { // Aim at tag 16 for testing // driverController.b().onTrue(new CMD_AimAtAprilTag(swerve, 16, 0.1)); - driverController.a().onTrue(intake.setState(SUB_Intake.State.ALGAE_GROUND)); - driverController.a().toggleOnFalse(intake.setState(SUB_Intake.State.STOWED)); + /*SequentialCommandGroup + driverController.a().onTrue(intake.setState(SUB_Intake.State.ALGAE_GROUND)); + driverController.a().toggleOnFalse(intake.setState(SUB_Intake.State.STOWED)); + } + */ + + driverController + .a() + .onTrue( + new ParallelCommandGroup( + intake.setState(SUB_Intake.State.ALGAE_GROUND), + swerve.driveToPose(FieldConstants.BLUE_TOP_TOP_LEFT))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/BuildConstants.java b/src/main/java/frc/robot/constants/BuildConstants.java index 693d304..0fefd68 100644 --- a/src/main/java/frc/robot/constants/BuildConstants.java +++ b/src/main/java/frc/robot/constants/BuildConstants.java @@ -12,12 +12,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "SwerveDrive2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 32; - public static final String GIT_SHA = "d45110397bccd5d27febd5a779bcede324db9cfa"; - public static final String GIT_DATE = "2025-01-10 17:46:32 EST"; - public static final String GIT_BRANCH = "Pesky-Branch"; - public static final String BUILD_DATE = "2025-01-10 19:40:19 EST"; - public static final long BUILD_UNIX_TIME = 1736556019556L; + public static final int GIT_REVISION = 38; + public static final String GIT_SHA = "167d038b6228fb29148746fa97f7517ad5143d57"; + public static final String GIT_DATE = "2025-01-10 19:51:22 EST"; + public static final String GIT_BRANCH = "main"; + public static final String BUILD_DATE = "2025-01-25 11:08:25 EST"; + public static final long BUILD_UNIX_TIME = 1737821305492L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/elevator/IO_ElevatorBase.java b/src/main/java/frc/robot/elevator/IO_ElevatorBase.java new file mode 100644 index 0000000..01983ee --- /dev/null +++ b/src/main/java/frc/robot/elevator/IO_ElevatorBase.java @@ -0,0 +1,48 @@ +// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs +// https://www.team2106.org + +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.elevator; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +/// +/// +/// // +public interface IO_ElevatorBase { + + @AutoLog + public static class ElevatorInputs implements LoggableInputs { + + public double heightM = 0.0; + public double velocityMPS = 0.0; + public double motorOneCurrent = 0.0; + public double motorTwoCurrent = 0.0; + + @Override + public void toLog(LogTable table) { + table.put("heightM", heightM); + table.put("velocityMPS", velocityMPS); + table.put("motorOneCurrent", motorOneCurrent); + table.put("motorTwoCurrent", motorTwoCurrent); + } + + @Override + public void fromLog(LogTable table) { + heightM = table.get("heightM", heightM); + velocityMPS = table.get("velocityMPS", velocityMPS); + motorOneCurrent = table.get("motorOneCurrent", motorOneCurrent); + motorTwoCurrent = table.get("motorTwoCurrent", motorTwoCurrent); + } + } + + /** Updates the set of loggable inputs. */ + public void updateInputs(ElevatorInputs inputs); + + public void setPositionM(double positionM); +} diff --git a/src/main/java/frc/robot/elevator/IO_ElevatorRealKracken.java b/src/main/java/frc/robot/elevator/IO_ElevatorRealKracken.java new file mode 100644 index 0000000..8eec6cb --- /dev/null +++ b/src/main/java/frc/robot/elevator/IO_ElevatorRealKracken.java @@ -0,0 +1,68 @@ +// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs +// https://www.team2106.org + +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.elevator; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +public class IO_ElevatorRealKracken implements IO_ElevatorBase { + + private final TalonFX motorOne; + private final TalonFX motorTwo; + private final MotionMagicVoltage motorRequest; + + // private SparkMax motorOne; + // private SparkMax motorTwo; + + public IO_ElevatorRealKracken() { + motorOne = new TalonFX(0); + motorTwo = new TalonFX(0); + motorRequest = new MotionMagicVoltage(0); + + var motorConfigs = new TalonFXConfiguration(); + + // One rotation = X meters of linear travel + // Calculate: (Sprocket Circumference in meters) / (Total Gear Reduction) + // Example: (0.1524 meters per rotation) / (5:1 gear ratio) = 0.03048 meters per motor rotation + double METERS_PER_MOTOR_ROTATION = 0.03048; + + var slot0Configs = motorConfigs.Slot0; + slot0Configs.kS = 0.25; // Add 0.25 V output to overcome static friction + slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kA = 0.01; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = 0.11; // An error of 1 rps results in 0.11 V output + slot0Configs.kI = 0; // no output for integrated error + slot0Configs.kD = 0; // no output for error derivative + + // Convert to meters + motorConfigs.Feedback.SensorToMechanismRatio = METERS_PER_MOTOR_ROTATION; + + var motionMagicConfigs = motorConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = 2.0; // meters per second + motionMagicConfigs.MotionMagicAcceleration = 4.0; // meters per second squared + motionMagicConfigs.MotionMagicJerk = 40.0; // meters per second cubed + + motorOne.getConfigurator().apply(motorConfigs); + motorTwo.getConfigurator().apply(motorConfigs); + } + + @Override + public void updateInputs(ElevatorInputs inputs) { + inputs.heightM = motorOne.getPosition().getValueAsDouble(); + inputs.velocityMPS = motorOne.getVelocity().getValueAsDouble(); + inputs.motorOneCurrent = motorOne.getSupplyCurrent().getValueAsDouble(); + inputs.motorTwoCurrent = motorTwo.getSupplyCurrent().getValueAsDouble(); + } + + @Override + public void setPositionM(double positionM) { + motorOne.setControl(motorRequest.withPosition(positionM)); + motorTwo.setControl(motorRequest.withPosition(positionM)); + } +} diff --git a/src/main/java/frc/robot/elevator/IO_ElevatorRealNeo.java b/src/main/java/frc/robot/elevator/IO_ElevatorRealNeo.java new file mode 100644 index 0000000..3ada9f3 --- /dev/null +++ b/src/main/java/frc/robot/elevator/IO_ElevatorRealNeo.java @@ -0,0 +1,38 @@ +// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs +// https://www.team2106.org + +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.elevator; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +public class IO_ElevatorRealNeo implements IO_ElevatorBase { + + private SparkMax motorOne; + private SparkMax motorTwo; + + public IO_ElevatorRealNeo() { + + motorOne = new SparkMax(0, MotorType.kBrushless); + motorTwo = new SparkMax(0, MotorType.kBrushless); + } + + @Override + public void updateInputs(ElevatorInputs inputs) { + inputs.heightM = motorOne.getEncoder().getPosition(); + inputs.velocityMPS = motorOne.getEncoder().getVelocity(); + inputs.motorOneCurrent = motorOne.getOutputCurrent(); + inputs.motorTwoCurrent = motorTwo.getOutputCurrent(); + } + + @Override + public void setPositionM(double speed) { + // motorOne.set(speed); + // motorTwo.set(speed); + // Fix this later + } +} diff --git a/src/main/java/frc/robot/elevator/SUB_Elevator.java b/src/main/java/frc/robot/elevator/SUB_Elevator.java new file mode 100644 index 0000000..3084438 --- /dev/null +++ b/src/main/java/frc/robot/elevator/SUB_Elevator.java @@ -0,0 +1,63 @@ +// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs +// https://www.team2106.org + +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class SUB_Elevator extends SubsystemBase { + + public enum State { + STOWED(0.0), + CORAL_STATION(1.0), + ALGAE_GROUND(0.1), + ALGAE_PROCESSOR(0.1), + ALGAE_BARGE(2.9), + L1_SCORING(0.5), + L2_SCORING(0.8), + L3_SCORING(1.2), + L4_SCORING(1.85); + + private final double heightM; + + State(double heightM) { + this.heightM = heightM; + } + + public double getHeightM() { + return heightM; + } + } + + private final IO_ElevatorBase io; + + private final IO_ElevatorBase.ElevatorInputs inputs = new IO_ElevatorBase.ElevatorInputs(); + + public SUB_Elevator(IO_ElevatorBase io) { + this.io = io; + } + + @Override + public void periodic() { + + // Update inputs + io.updateInputs(inputs); + + // Process inputs + Logger.processInputs("Elevator", inputs); + } + + public Command setState(State state) { + return new InstantCommand( + () -> { + io.setPositionM(state.getHeightM()); + }); + } +}