From d66622d683dc21866d8b1ce907d52805ffd9f46c Mon Sep 17 00:00:00 2001 From: junocapra <157174638+junocapra@users.noreply.github.com> Date: Wed, 5 Feb 2025 18:18:12 -0500 Subject: [PATCH] Coral intake + algae pivot (#27) * juno's intake * IntakeAdvantageKit * Intake Advantagekit Done * Edited intake a bit * Intake edits * Formatting fixes * Worked on coral pivot * fjioe * continued algae pivot * htuiew * gfi * jkjlkjlk --------- Co-authored-by: Ishan1522 Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- build.gradle | 1 + src/main/java/frc/robot/RobotContainer.java | 39 ++++++ .../commands/algaePivot/ManualAlgaePivot.java | 35 ++++++ .../java/frc/robot/commands/intake/Eject.java | 29 +++++ .../frc/robot/commands/intake/Intake.java | 29 +++++ .../subsystems/AlgaePivot/AlgaeConstants.java | 31 +++++ .../AlgaePivot/AlgaePivotInterface.java | 36 ++++++ .../AlgaePivot/AlgaePivotSubsystem.java | 30 +++++ .../AlgaePivot/PhysicalAlgaePivot.java | 115 ++++++++++++++++++ .../AlgaePivot/SimulatedAlgaePivot.java | 54 ++++++++ .../elevator/SimulatedElevator.java | 70 ----------- .../subsystems/intake/IntakeConstants.java | 15 +++ .../subsystems/intake/IntakeInterface.java | 23 ++++ .../subsystems/intake/IntakeSubsystem.java | 27 ++++ .../subsystems/intake/PhysicalIntake.java | 46 +++++++ .../subsystems/intake/SimulatedIntake.java | 35 ++++++ 16 files changed, 545 insertions(+), 70 deletions(-) create mode 100644 src/main/java/frc/robot/commands/algaePivot/ManualAlgaePivot.java create mode 100644 src/main/java/frc/robot/commands/intake/Eject.java create mode 100644 src/main/java/frc/robot/commands/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaePivot/AlgaeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotInterface.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaePivot/PhysicalAlgaePivot.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaePivot/SimulatedAlgaePivot.java delete mode 100644 src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeInterface.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java diff --git a/build.gradle b/build.gradle index cc906ab..912795e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,3 +1,4 @@ + plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.2.1" diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f652153..d5dac11 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,14 +11,21 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SimulationConstants; +import frc.robot.commands.algaePivot.ManualAlgaePivot; import frc.robot.commands.autodrive.AutoAlign; import frc.robot.commands.drive.DriveCommand; +import frc.robot.commands.intake.Eject; +import frc.robot.commands.intake.Intake; import frc.robot.extras.simulation.field.SimulatedField; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; import frc.robot.extras.util.JoystickUtil; +import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem; +import frc.robot.subsystems.algaePivot.PhysicalAlgaePivot; +import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.intake.PhysicalIntake; import frc.robot.subsystems.swerve.SwerveConstants; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; @@ -42,6 +49,9 @@ public class RobotContainer { private final CommandXboxController operatorController = new CommandXboxController(1); private final CommandXboxController driverController = new CommandXboxController(0); + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(new PhysicalIntake()); + private final AlgaePivotSubsystem algaePivotSubsystem = + new AlgaePivotSubsystem(new PhysicalAlgaePivot()); // Simulation, we store them here in the robot container // private final SimulatedField simulatedArena; @@ -171,6 +181,35 @@ private void configureButtonBindings() { Trigger driverRightDirectionPad = new Trigger(driverController.pov(90)); Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270)); + driverController.a().whileTrue(new Intake(intakeSubsystem)); + driverController.b().whileTrue(new Eject(intakeSubsystem)); + driverController + .x() + .whileTrue(new ManualAlgaePivot(algaePivotSubsystem, operatorController::getLeftY)); + + // // autodrive + // Trigger driverAButton = new Trigger(driverController::getAButton); + // lol whatever + // // intake + // Trigger operatorLeftTrigger = new Trigger(()->operatorController.getLeftTriggerAxis() > 0.2); + // Trigger operatorLeftBumper = new Trigger(operatorController::getLeftBumper); + // // amp and speaker + // Trigger operatorBButton = new Trigger(operatorController::getBButton); + // Trigger operatorRightBumper = new Trigger(operatorController::getRightBumper); + // Trigger operatorRightTrigger = new Trigger(()->operatorController.getRightTriggerAxis() > + // 0.2); + // Trigger driverRightTrigger = new Trigger(()->driverController.getRightTriggerAxis() > 0.2); + + // // manual pivot and intake rollers + // Trigger operatorAButton = new Trigger(operatorController::getAButton); + // Trigger operatorXButton = new Trigger(operatorController::getXButton); + // Trigger operatorYButton = new Trigger(operatorController::getYButton); + // DoubleSupplier operatorRightStickY = operatorController::getRightY; + // // unused + // Trigger operatorUpDirectionPad = new Trigger(()->operatorController.getPOV() == 0); + // Trigger operatorLeftDirectionPad = new Trigger(()->operatorController.getPOV() == 270); + // Trigger operatorDownDirectionPad = new Trigger(()->operatorController.getPOV() == 180); + // Trigger driverLeftTrigger = new Trigger(()->driverController.getLeftTriggerAxis() > 0.2); Trigger driverLeftBumper = new Trigger(driverController.leftBumper()); // DRIVER BUTTONS diff --git a/src/main/java/frc/robot/commands/algaePivot/ManualAlgaePivot.java b/src/main/java/frc/robot/commands/algaePivot/ManualAlgaePivot.java new file mode 100644 index 0000000..5a911f1 --- /dev/null +++ b/src/main/java/frc/robot/commands/algaePivot/ManualAlgaePivot.java @@ -0,0 +1,35 @@ +package frc.robot.commands.algaePivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algaePivot.AlgaeConstants; +import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem; +import java.util.function.DoubleSupplier; + +public class ManualAlgaePivot extends Command { + private final AlgaePivotSubsystem algaePivotSubsystem; + private final DoubleSupplier speed; + + public ManualAlgaePivot(AlgaePivotSubsystem algaePivotSubsystem, DoubleSupplier speed) { + this.algaePivotSubsystem = algaePivotSubsystem; + this.speed = speed; + addRequirements(algaePivotSubsystem); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + algaePivotSubsystem.setAlgaeSpeed(speed.getAsDouble()); + } + + @Override + public void end(boolean interrupted) { + algaePivotSubsystem.setAlgaeSpeed(AlgaeConstants.ALGAE_NEUTRAL_SPEED); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/intake/Eject.java b/src/main/java/frc/robot/commands/intake/Eject.java new file mode 100644 index 0000000..d588c9b --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/Eject.java @@ -0,0 +1,29 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.intake.IntakeSubsystem; + +public class Eject extends Command { + private final IntakeSubsystem intakeSubsystem; + + public Eject(IntakeSubsystem intakeSubsystem) { + this.intakeSubsystem = intakeSubsystem; + addRequirements(this.intakeSubsystem); + } + + @Override + public void execute() { + intakeSubsystem.setIntakeSpeed(IntakeConstants.EJECT_SPEED); + } + + @Override + public void end(boolean interrupted) { + intakeSubsystem.setIntakeSpeed(0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/intake/Intake.java b/src/main/java/frc/robot/commands/intake/Intake.java new file mode 100644 index 0000000..7ef8760 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/Intake.java @@ -0,0 +1,29 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.intake.IntakeSubsystem; + +public class Intake extends Command { + private final IntakeSubsystem intakeSubsystem; + + public Intake(IntakeSubsystem intakeSubsystem) { + this.intakeSubsystem = intakeSubsystem; + addRequirements(this.intakeSubsystem); + } + + @Override + public void execute() { + intakeSubsystem.setIntakeSpeed(IntakeConstants.INTAKE_SPEED); + } + + @Override + public void end(boolean interrupted) { + intakeSubsystem.setIntakeSpeed(0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaeConstants.java b/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaeConstants.java new file mode 100644 index 0000000..93f1073 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaeConstants.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.algaePivot; + +import com.ctre.phoenix6.signals.SensorDirectionValue; + +public class AlgaeConstants { + public static final int ALGAE_PIVOT_MOTOR_ID = 0; + public static final int ALGAE_ENCODER_MOTOR_ID = 0; + + public static final SensorDirectionValue ALGAE_ENCODER_REVERSED = + SensorDirectionValue.Clockwise_Positive; + public static final double MAX_VELOCITY_ROTATIONS_PER_SECOND = 0.0; + public static final double MAX_ACCELERATION_ROTATIONS_PER_SECOND = 0.0; + + public static final double MAX_ANGLE = 0.0; + public static final double MIN_ANGLE = 0.0; + + public static final double ANGLE_ZERO = 0.0; + public static final double PIVOT_P = 0.0; + public static final double PIVOT_I = 0.0; + public static final double PIVOT_D = 0.0; + public static final double PIVOT_G = 0.0; + + public static final double ALGAE_GEAR_RATIO = 0.0; + public static final double ALGAE_PIVOT_MASS = 0.0; + public static final double ALGAE_PIVOT_LENGTH = 0.0; + + public static final double ALGAE_PIVOT_ANGLE = 0.0; + public static final double ALGAE_PIVOT_SPEED = 0.0; + public static final double ALGAE_VOLTAGE = 0.0; + public static final double ALGAE_NEUTRAL_SPEED = 0.0; +} diff --git a/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotInterface.java b/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotInterface.java new file mode 100644 index 0000000..4de42f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotInterface.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.algaePivot; + +import org.littletonrobotics.junction.AutoLog; + +public interface AlgaePivotInterface { + @AutoLog + public static class AlgaePivotInputs { + public boolean isConnected = true; + public double algaeAngle = 0.0; + public double algaeVoltage = 0.0; + public double algaeVelocity = 0.0; + public double algaeTemp = 0.0; + public double algaeSupplyCurrentAmps = 0.0; + public double algaeTorqueCurrentAmps = 0.0; + } + + default void updateInputs(AlgaePivotInputs inputs) {} + + default void setAlgaeSpeed(double speed) {} + + default void setAlgaeAngle(double angle) {} + + default void setAlgaeVoltage(double voltage) {} + + default boolean isPivotWithinAcceptapleError() { + return true; + } + + default double getAlgaeAngle() { + return 0.0; + } + + default double getAlgaePivotTarget() { + return 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotSubsystem.java new file mode 100644 index 0000000..42b5f42 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotSubsystem.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.algaePivot; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class AlgaePivotSubsystem extends SubsystemBase { + private final AlgaePivotInterface algaePivotInterface; + private final AlgaePivotInputsAutoLogged inputs = new AlgaePivotInputsAutoLogged(); + + public AlgaePivotSubsystem(AlgaePivotInterface algaePivotInterface) { + this.algaePivotInterface = algaePivotInterface; + } + + public void setAlgaeSpeed(double speed) { + algaePivotInterface.setAlgaeSpeed(speed); + } + + public void setAlgaeAngle(double angle) { + algaePivotInterface.setAlgaeAngle(angle); + } + + public void setAlgaeVoltage(double voltage) { + algaePivotInterface.setAlgaeVoltage(voltage); + } + + public void periodic() { + algaePivotInterface.updateInputs(inputs); + Logger.processInputs("AlgaePivotSubsystem/", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/AlgaePivot/PhysicalAlgaePivot.java b/src/main/java/frc/robot/subsystems/AlgaePivot/PhysicalAlgaePivot.java new file mode 100644 index 0000000..df315d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaePivot/PhysicalAlgaePivot.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.algaePivot; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.HardwareConstants; + +public class PhysicalAlgaePivot implements AlgaePivotInterface { + private final TalonFX algaeMotor; + private final CANcoder algaeEncoder; + private final TalonFXConfiguration algaeMotorConfig; + private final CANcoderConfiguration algaeEncoderConfig; + private final StatusSignal algaeVoltage; + private final StatusSignal algaeVelocity; + private StatusSignal algaeAngle; + private final StatusSignal algaeSupplyCurrent; + private final StatusSignal algaeStatorCurrent; + private final MotionMagicVoltage mmPositionRequest; + private double algaeTargetAngle; + private final VoltageOut voltageOut; + + public PhysicalAlgaePivot() { + algaeMotor = new TalonFX(AlgaeConstants.ALGAE_PIVOT_MOTOR_ID); + algaeEncoder = new CANcoder(AlgaeConstants.ALGAE_ENCODER_MOTOR_ID); + algaeMotorConfig = new TalonFXConfiguration(); + algaeEncoderConfig = new CANcoderConfiguration(); + mmPositionRequest = new MotionMagicVoltage(0); + + algaeVoltage = algaeMotor.getMotorVoltage(); + algaeVelocity = algaeMotor.getVelocity(); + algaeAngle = algaeEncoder.getAbsolutePosition(); + algaeSupplyCurrent = algaeMotor.getSupplyCurrent(); + algaeStatorCurrent = algaeMotor.getStatorCurrent(); + voltageOut = new VoltageOut(0); + + algaeEncoderConfig.MagnetSensor.MagnetOffset = -AlgaeConstants.ANGLE_ZERO; + algaeEncoderConfig.MagnetSensor.SensorDirection = AlgaeConstants.ALGAE_ENCODER_REVERSED; + algaeEncoder.getConfigurator().apply(algaeEncoderConfig, HardwareConstants.TIMEOUT_S); + + algaeMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + algaeMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + algaeMotorConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; + + algaeMotorConfig.Slot0.kP = AlgaeConstants.PIVOT_P; + algaeMotorConfig.Slot0.kI = AlgaeConstants.PIVOT_I; + algaeMotorConfig.Slot0.kD = AlgaeConstants.PIVOT_D; + algaeMotorConfig.Slot0.kG = AlgaeConstants.PIVOT_G; + algaeMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + + algaeMotorConfig.MotionMagic.MotionMagicAcceleration = + AlgaeConstants.MAX_VELOCITY_ROTATIONS_PER_SECOND; + algaeMotorConfig.MotionMagic.MotionMagicCruiseVelocity = + AlgaeConstants.MAX_ACCELERATION_ROTATIONS_PER_SECOND; + + algaeMotorConfig.ClosedLoopGeneral.ContinuousWrap = true; + + algaeMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + algaeMotorConfig.Feedback.FeedbackRemoteSensorID = algaeEncoder.getDeviceID(); + + algaeMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = AlgaeConstants.MAX_ANGLE; + algaeMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = AlgaeConstants.MIN_ANGLE; + algaeMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + algaeMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, algaeAngle, algaeVelocity, algaeVoltage, algaeSupplyCurrent, algaeStatorCurrent); + } + + @Override + public void updateInputs(AlgaePivotInputs inputs) { + inputs.algaeAngle = algaeAngle.getValueAsDouble(); + inputs.algaeVelocity = algaeVelocity.getValueAsDouble(); + inputs.algaeVoltage = algaeVoltage.getValueAsDouble(); + } + + @Override + public void setAlgaeSpeed(double speed) { + algaeMotor.set(speed); + } + + @Override + public void setAlgaeAngle(double angle) { + algaeTargetAngle = angle; + algaeMotor.setControl(mmPositionRequest.withPosition(angle)); + } + + @Override + public void setAlgaeVoltage(double voltage) { + algaeMotor.setControl(voltageOut.withOutput(voltage)); + } + + @Override + public double getAlgaeAngle() { + algaeAngle.refresh(); + return algaeAngle.getValueAsDouble(); + } + + @Override + public double getAlgaePivotTarget() { + return algaeTargetAngle; + } +} diff --git a/src/main/java/frc/robot/subsystems/AlgaePivot/SimulatedAlgaePivot.java b/src/main/java/frc/robot/subsystems/AlgaePivot/SimulatedAlgaePivot.java new file mode 100644 index 0000000..41b1ca5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaePivot/SimulatedAlgaePivot.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.algaePivot; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants.HardwareConstants; + +public class SimulatedAlgaePivot implements AlgaePivotInterface { + private final double algaeGearing = AlgaeConstants.ALGAE_GEAR_RATIO; + private final double algaePivotMass = AlgaeConstants.ALGAE_PIVOT_MASS; + private final double algaePivotLength = AlgaeConstants.ALGAE_PIVOT_LENGTH; + private SingleJointedArmSim algaePivotSim = + new SingleJointedArmSim( + DCMotor.getKrakenX60(2), algaeGearing, algaePivotMass, algaePivotLength, 0, 0, true, 0); + + private final double armKS = 0.0; + private final double armKG = AlgaeConstants.PIVOT_G; + private final double armKV = 0.0; + private final ArmFeedforward armFeedForward = new ArmFeedforward(armKS, armKG, armKV); + private final Constraints algaeConstraints = new Constraints(0, 0); + private final ProfiledPIDController algaePivotController = + new ProfiledPIDController(0, 0, 0, algaeConstraints); + + private double appliedVolts = 0.0; + private double position = 0.0; + private double velocity = 0.0; + private double supplyCurrentAmps = 0.0; + + @Override + public void updateInputs(AlgaePivotInputs inputs) { + algaePivotSim.update(HardwareConstants.TIMEOUT_S); + + inputs.algaeVelocity = Units.radiansToRotations(algaePivotSim.getAngleRads()); + inputs.algaeAngle = Units.radiansToRotations(algaePivotSim.getVelocityRadPerSec()); + inputs.algaeSupplyCurrentAmps = algaePivotSim.getCurrentDrawAmps(); + inputs.algaeVoltage = appliedVolts; + } + + @Override + public void setAlgaeVoltage(double voltage) { + appliedVolts = voltage; + algaePivotSim.setInputVoltage(voltage); + } + + @Override + public void setAlgaeAngle(double angle) { + double currentAlgaePivotAngleRots = Units.radiansToRotations(algaePivotSim.getAngleRads()); + double armFF = armFeedForward.calculate(angle, algaePivotController.getSetpoint().velocity); + setAlgaeVoltage(algaePivotController.calculate(angle, currentAlgaePivotAngleRots) + armFF); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java deleted file mode 100644 index dc59614..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java +++ /dev/null @@ -1,70 +0,0 @@ -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. - -// package frc.robot.subsystems.elevator; - -// import edu.wpi.first.wpilibj.simulation.ElevatorSim; -// import edu.wpi.first.math.controller.ElevatorFeedforward; -// import edu.wpi.first.math.controller.PIDController; -// import edu.wpi.first.math.system.plant.DCMotor; -// import edu.wpi.first.wpilibj.Encoder; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import frc.robot.subsystems.elevator.ElevatorInterface.ElevatorInputs; - -// public class SimulatedElevator implements ElevatorInterface { -// private final ElevatorSim m_elevatorSim; -// private final Encoder m_encoder; -// private final PIDController m_pidController; -// private final ElevatorFeedforward m_feedforward; -// private double currentVolts; - -// public SimulatedElevator() { -// m_elevatorSim = new ElevatorSim( //we dont know the -// design............................................................. -// DCMotor.getVex775Pro(2), -// 10.0, -// 7.5, -// 0.75, -// 0.0, -// 2.056, -// true, 0, null -// ); -// m_encoder = new Encoder(0, 1); -// m_pidController = new -// PIDController(ElevatorConstants.ELEVATOR_P,ElevatorConstants.ELEVATOR_I,ElevatorConstants.ELEVATOR_D); -// m_feedforward = new -// ElevatorFeedforward(ElevatorConstants.ELEVATOR_S,ElevatorConstants.ELEVATOR_V,ElevatorConstants.ELEVATOR_G,ElevatorConstants.ELEVATOR_A); -// currentVolts = 0.0; -// } - -// public void updateInputs(ElevatorInputs inputs) { -// m_elevatorSim.update(0.02); -// inputs.leaderMotorPosition = m_encoder.getDistance(); -// inputs.followerMotorPosition = m_encoder.getDistance(); -// } - -// public double getElevatorPosition() { -// return m_encoder.getDistance(); -// } - -// public void setElevatorPosition(double position) { -// m_pidController.setSetpoint(position); -// double output = m_pidController.calculate(m_encoder.getDistance()); -// double feedforward = m_feedforward.calculate(m_pidController.getSetpoint()); -// setVolts(output + feedforward); -// } - -// @Override -// public void setVolts(double volts){ -// double output = m_pidController.calculate(m_encoder.getDistance()); -// double feedforward = m_feedforward.calculate(m_pidController.getSetpoint()); -// currentVolts = output + feedforward; -// m_elevatorSim.setInputVoltage(currentVolts); -// } - -// public double getVolts() { -// return currentVolts; -// } -// } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..cd1e4b8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.intake; + +public final class IntakeConstants { + public static final int INTAKE_MOTOR_ID = 0; + + public static final int INTAKE_SPEED = 0 - 9; + public static final int EJECT_SPEED = 0 - 9; + + // the max amount of stator and supply current allowed in the motor, respectively + public static final double INTAKE_STATOR_LIMIT = 0.0; + public static final double INTAKE_SUPPLY_LIMIT = 0.0; + // Enables stator and supply current limits, respectively + public static final boolean INTAKE_STATOR_LIMIT_ENABLE = true; + public static final boolean INTAKE_SUPPLY_LIMIT_ENABLE = true; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java b/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java new file mode 100644 index 0000000..16348cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeInterface { + + @AutoLog + public static class IntakeInputs { + public boolean isConnected = true; + public double intakeVelocity = 0.0; + public double intakeTemp = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + default void updateInputs(IntakeInputs inputs) {} + + default void setIntakeSpeed(double speed) {} + + default double getIntakeSpeed() { + return 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java new file mode 100644 index 0000000..b0b8f6b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class IntakeSubsystem extends SubsystemBase { + private IntakeInterface intakeInterface; + private IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged(); + + public IntakeSubsystem(IntakeInterface intakeInterface) { + this.intakeInterface = intakeInterface; + } + + public void setIntakeSpeed(double speed) { + intakeInterface.setIntakeSpeed(speed); + } + + @Override + public void periodic() { + intakeInterface.updateInputs(inputs); + Logger.processInputs("IntakeSubsystem/", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java new file mode 100644 index 0000000..ca91da6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.HardwareConstants; + +public class PhysicalIntake implements IntakeInterface { + private final TalonFX motor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + private final StatusSignal intakeVelocity = motor.getVelocity(); + private final TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); + + public PhysicalIntake() { + intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + intakeConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; + + intakeConfig.CurrentLimits.StatorCurrentLimit = IntakeConstants.INTAKE_STATOR_LIMIT; + intakeConfig.CurrentLimits.StatorCurrentLimitEnable = + IntakeConstants.INTAKE_STATOR_LIMIT_ENABLE; + intakeConfig.CurrentLimits.SupplyCurrentLimit = IntakeConstants.INTAKE_SUPPLY_LIMIT; + intakeConfig.CurrentLimits.StatorCurrentLimitEnable = + IntakeConstants.INTAKE_SUPPLY_LIMIT_ENABLE; + + motor.getConfigurator().apply(intakeConfig); + } + + @Override + public void updateInputs(IntakeInputs intakeInputs) { + intakeInputs.intakeVelocity = intakeVelocity.getValueAsDouble(); + } + + @Override + public void setIntakeSpeed(double speed) { + motor.set(speed); + } + + @Override + public double getIntakeSpeed() { + motor.getVelocity().refresh(); + return motor.getVelocity().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java new file mode 100644 index 0000000..3f53f28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class SimulatedIntake implements IntakeInterface { + DCMotorSim simIntake = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), 0.01, 1), + DCMotor.getFalcon500(1), + 0); + + private double intakeAppliedVolts = 0.0; + + public SimulatedIntake() {} + + @Override + public void updateInputs(IntakeInputs intakeInputs) { + simIntake.update(0.02); + + intakeInputs.intakeVelocity = + RadiansPerSecond.of(simIntake.getAngularVelocityRadPerSec()).in(RotationsPerSecond); + intakeInputs.currentAmps = simIntake.getCurrentDrawAmps(); + intakeInputs.appliedVolts = intakeAppliedVolts; + } + + @Override + public double getIntakeSpeed() { + return RadiansPerSecond.of(simIntake.getAngularVelocityRadPerSec()).in(RotationsPerSecond); + } +}