From 19b4121c08099b08c4756a7ef81fc4d1feead008 Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Wed, 15 Jan 2025 16:53:09 -0500 Subject: [PATCH] Intake Advantagekit Done --- src/main/java/frc/robot/RobotContainer.java | 22 ++-- .../java/frc/robot/commands/drive/Eject.java | 27 +++++ .../java/frc/robot/commands/drive/Intake.java | 26 +++++ .../frc/robot/commands/drive/XSwerve.java | 37 ------ .../robot/subsystems/elevator/Elevator.java | 43 +++++++ .../elevator/ElevatorConstants.java | 32 ++++++ .../elevator/ElevatorInterface.java | 33 ++++++ .../subsystems/elevator/PhysicalElevator.java | 105 ++++++++++++++++++ .../elevator/SimulatedElevator.java | 53 +++++++++ .../subsystems/intake/IntakeConstants.java | 3 + 10 files changed, 333 insertions(+), 48 deletions(-) create mode 100644 src/main/java/frc/robot/commands/drive/Eject.java create mode 100644 src/main/java/frc/robot/commands/drive/Intake.java delete mode 100644 src/main/java/frc/robot/commands/drive/XSwerve.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/Elevator.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e473be1..8d48b3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,12 +10,17 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.SimulationConstants; import frc.robot.commands.drive.DriveCommand; +import frc.robot.commands.drive.Eject; +import frc.robot.commands.drive.Intake; import frc.robot.extras.simulation.field.SimulatedField; +import frc.robot.extras.simulation.mechanismSim.IntakeSimulation.IntakeSide; 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.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; @@ -43,6 +48,7 @@ public class RobotContainer { // private final Pivot pivot = new Pivot(new PivotIOTalonFX()); // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX()); private final CommandXboxController driverController = new CommandXboxController(0); + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(new PhysicalIntake()); // Simulation, we store them here in the robot container // private final SimulatedField simulatedArena; @@ -179,6 +185,10 @@ private void configureButtonBindings() { Trigger driverDownDirectionPad = new Trigger(driverController.pov(180)); Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270)); + driverController.a().whileTrue(new Intake(intakeSubsystem)); + driverController.b().whileTrue(new Eject(intakeSubsystem)); + + // // autodrive // Trigger driverAButton = new Trigger(driverController::getAButton); // lol whatever @@ -211,17 +221,7 @@ private void configureButtonBindings() { // // driving - Command driveCommand = - new DriveCommand( - swerveDrive, - visionSubsystem, - driverLeftStick[1], - driverLeftStick[0], - () -> JoystickUtil.modifyAxis(driverRightStickX, 3), - () -> !driverRightBumper.getAsBoolean(), - () -> driverLeftBumper.getAsBoolean()); - swerveDrive.setDefaultCommand(driveCommand); - + // // shooterSubsystem.setDefaultCommand(new FlywheelSpinUpAuto(shooterSubsystem, // visionSubsystem)); diff --git a/src/main/java/frc/robot/commands/drive/Eject.java b/src/main/java/frc/robot/commands/drive/Eject.java new file mode 100644 index 0000000..ae5b3a5 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/Eject.java @@ -0,0 +1,27 @@ +package frc.robot.commands.drive; + +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); + } + + public void execute(){ + intakeSubsystem.setIntakeSpeed(IntakeConstants.ejectSpeed); + } + + public void end(){ + intakeSubsystem.setIntakeSpeed(0); + } + + public boolean isFinished(){ + return false; + } +} diff --git a/src/main/java/frc/robot/commands/drive/Intake.java b/src/main/java/frc/robot/commands/drive/Intake.java new file mode 100644 index 0000000..68ccfb6 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/Intake.java @@ -0,0 +1,26 @@ +package frc.robot.commands.drive; + +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); + } + + public void execute(){ + intakeSubsystem.setIntakeSpeed(IntakeConstants.intakeSpeed); + } + + public void end(){ + intakeSubsystem.setIntakeSpeed(0); + } + + public boolean isFinished(){ + return false; + } +} diff --git a/src/main/java/frc/robot/commands/drive/XSwerve.java b/src/main/java/frc/robot/commands/drive/XSwerve.java deleted file mode 100644 index 636ab4d..0000000 --- a/src/main/java/frc/robot/commands/drive/XSwerve.java +++ /dev/null @@ -1,37 +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.commands.drive; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swerve.SwerveConstants; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class XSwerve extends Command { - /** Creates a new XSwerve. */ - public XSwerve() { - SwerveConstants swerveConstants; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - if - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..2ef428e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,43 @@ +// 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.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Elevator extends SubsystemBase { + /** Creates a new elevator. */ + private ElevatorInterface elevatorInterface; + + private ElevatorInputsAutoLogged inputs = new ElevatorInputsAutoLogged(); + + public Elevator(ElevatorInterface elevatorInterface) { + this.elevatorInterface = elevatorInterface; + } + + public double getElevatorPosition() { + return elevatorInterface.getElevatorPosition(); + } + + public double getVolts() { + return elevatorInterface.getVolts(); + } + + public void setElevatorPosition(double position) { + elevatorInterface.setElevatorPosition(position); + Logger.recordOutput("Elevator/Position", getElevatorPosition()); + } + + public void setVolts(double volts) { + elevatorInterface.setVolts(volts); + Logger.recordOutput("Elevator/Volts", getVolts()); + } + + @Override + public void periodic() { + elevatorInterface.updateInputs(inputs); + Logger.processInputs("Elevator/", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..759e79e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,32 @@ +// 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; + +/** Add your docs here. */ +public class ElevatorConstants { + public static final int ELEVATOR_LEADER_MOTOR_ID = 0 - 9; + public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 0 - 9; + + public static final double ELEVATOR_P = 0 - 9; + public static final double ELEVATOR_I = 0 - 9; + public static final double ELEVATOR_D = 0 - 9; + public static final double ELEVATOR_S = 0 - 9; + public static final double ELEVATOR_V = 0 - 9; + public static final double ELEVATOR_A = 0 - 9; + public static final double ELEVATOR_G = 0 - 9; + + + public static final double DRUM_RADIUS = 0 - 9; + public static final double ELEVATOR_GEAR_RATIO = 2; + public static final double ELEVATOR_CARRIAGE_MASS = 0 - 9; + public static final double ENCODER_CONVERSION_FACTOR = 71.81; + public static final double MIN_HEIGHT = 0; + public static final double MAX_HEIGHT = 0 - 9; + + public static final double STATOR_CURRENT_LIMIT = 0 - 9; + public static final double SUPPLY_CURRENT_LIMIT = 0 - 9; + public static final boolean STATOR_CURRENT_LIMIT_ENABLE = true; + public static final boolean SUPPLY_CURRENT_LIMIT_ENABLE = true; + } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java new file mode 100644 index 0000000..ee9a456 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java @@ -0,0 +1,33 @@ +// 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 org.littletonrobotics.junction.AutoLog; + +public interface ElevatorInterface { + /** Creates a new ElevatorInterface. */ + @AutoLog + public static class ElevatorInputs { // For values + public double leaderMotorPosition = 0.0; + public double followerMotorPosition = 0.0; + public double leaderMotorVoltage = 0.0; + public double followerMotorVoltage = 0.0; + + } + + public default void updateInputs(ElevatorInputs inputs) {} + + public default double getElevatorPosition() { + return 0.0; + } + + public default void setElevatorPosition(double position) {} + + public default void setVolts(double volts) {} + + public default double getVolts() { + return 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java new file mode 100644 index 0000000..ea826ed --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -0,0 +1,105 @@ +// 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 com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.HardwareConstants; +import frc.robot.subsystems.elevator.ElevatorInterface.ElevatorInputs; + +public class PhysicalElevator implements ElevatorInterface { + + /** Creates a new ElevatorHardware. */ + private final TalonFX leaderMotor = new TalonFX(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID); + private final TalonFX followerMotor = new TalonFX(ElevatorConstants.ELEVATOR_FOLLOWER_MOTOR_ID); + private final Follower followerMotorControl = new Follower(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID, true); + + StatusSignal leaderPosition; + StatusSignal followerPosition; + StatusSignal leaderAppliedVoltage; + StatusSignal followerAppliedVoltage; + + public PhysicalElevator() { + leaderPosition = leaderMotor.getRotorPosition(); + leaderAppliedVoltage = leaderMotor.getMotorVoltage(); + followerPosition = followerMotor.getRotorPosition(); + followerAppliedVoltage = followerMotor.getMotorVoltage(); + + followerMotor.setControl(followerMotorControl); + + TalonFXConfiguration elevatorConfig = new TalonFXConfiguration(); + + elevatorConfig.Slot0.kP = ElevatorConstants.ELEVATOR_P; + elevatorConfig.Slot0.kI = ElevatorConstants.ELEVATOR_I; + elevatorConfig.Slot0.kD = ElevatorConstants.ELEVATOR_D; + elevatorConfig.Slot0.kS = ElevatorConstants.ELEVATOR_S; + elevatorConfig.Slot0.kV = ElevatorConstants.ELEVATOR_V; + elevatorConfig.Slot0.kA = ElevatorConstants.ELEVATOR_A; + elevatorConfig.Slot0.kG = ElevatorConstants.ELEVATOR_G; + + + elevatorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + elevatorConfig.CurrentLimits.StatorCurrentLimit = ElevatorConstants.STATOR_CURRENT_LIMIT; + elevatorConfig.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.SUPPLY_CURRENT_LIMIT; + elevatorConfig.CurrentLimits.StatorCurrentLimitEnable = ElevatorConstants.STATOR_CURRENT_LIMIT_ENABLE; + elevatorConfig.CurrentLimits.SupplyCurrentLimitEnable = ElevatorConstants.STATOR_CURRENT_LIMIT_ENABLE; + + elevatorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + leaderMotor.getConfigurator().apply(elevatorConfig); + followerMotor.getConfigurator().apply(elevatorConfig); + + BaseStatusSignal.setUpdateFrequencyForAll(HardwareConstants.SIGNAL_FREQUENCY, leaderPosition, leaderAppliedVoltage, followerPosition, followerAppliedVoltage); + } + public void updateInputs(ElevatorInputs inputs) { + inputs.leaderMotorPosition = leaderPosition.getValueAsDouble(); + inputs.leaderMotorVoltage = leaderAppliedVoltage.getValueAsDouble(); + inputs.followerMotorPosition = followerPosition.getValueAsDouble(); + inputs.followerMotorVoltage = followerAppliedVoltage.getValueAsDouble(); + } + + public double getElevatorPosition() { + return leaderPosition.getValueAsDouble(); + } + public void setElevatorLevel(int number) { + switch(number) { + case 1: + + case 2: + + case 3: + + case 4: + } + } + + public void setElevatorPosition(double position) { + leaderMotor.setPosition(position); + followerMotor.setPosition(position); + } + + public void setVolts(double volts) { + leaderMotor.setVoltage(volts); + followerMotor.setVoltage(volts); + } + + public double getVolts() { + return leaderMotor.getMotorVoltage().getValueAsDouble(); + } + + + + +} diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java new file mode 100644 index 0000000..16c8227 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -0,0 +1,53 @@ +// 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.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SimulatedElevator implements ElevatorInterface { + private ElevatorSim elevatorSim = + new ElevatorSim( + DCMotor.getFalcon500(2), + ElevatorConstants.ELEVATOR_GEAR_RATIO, + ElevatorConstants.ELEVATOR_CARRIAGE_MASS, + ElevatorConstants.DRUM_RADIUS, + ElevatorConstants.MIN_HEIGHT, + ElevatorConstants.MAX_HEIGHT, + true, + 0.0); + private PIDController simPID; + private double currentVolts; + public SimulatedElevator() { + simPID = + new PIDController( + ElevatorConstants.ELEVATOR_P, + ElevatorConstants.ELEVATOR_I, + ElevatorConstants.ELEVATOR_D);} + public void updateInputs(ElevatorInputs inputs) { + inputs.leaderMotorPosition = getElevatorPosition(); + inputs.followerMotorPosition = getElevatorPosition(); + } + public void setElevatorPosition(double position) { + setVolts(simPID.calculate(getElevatorPosition(), position)); + } + + public double getElevatorPosition() { + return elevatorSim.getPositionMeters(); + } + + public void setVolts(double volts) { + currentVolts = simPID.calculate(volts); + 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 index e8e7b4a..86284e7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -2,4 +2,7 @@ public final class IntakeConstants { public static final int intakeMotorID = 0; + + public static final int intakeSpeed = 0 - 9; + public static final int ejectSpeed = 0 - 9; }