diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 9a02b8b..d85d538 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -616,8 +616,29 @@ public static class ManipulatorConstants{ public static final double ROLLER_VOLTAGE = 8; } + public static class ElevatorConstants { + public static final int ELV1_ID = 11; + public static final int ELV2_ID = 12; + + public static final double kP = 6; + public static final double kI = 0; + public static final double kD = 0; + + public static final double kS = 0.475; + public static final double kV = 0; + public static final double kG = 0.625; + + public static final double MIN_DIST = 5; //Ask Charlie + public static final double MAX_DIST = 55; //Ask Charlie + + public static final double GEAR_RATIO = 12.5; + public static final double SPOOL_CIRCUMFERENCE = 3 * Math.PI; + + public static final int CURRENT_LIMIT = 80; + } + public static class BalanceConstants{ - public static final double turnKP = 0.05; + public static final double turnKP = 6; public static final double turnKI = 0; public static final double turnKD = .005; public static final double TURN_TOLERANCE = 1.5; diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index b8adfe7..232a6ef 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -27,6 +27,7 @@ import frc.team3128.common.hardware.input.NAR_XboxController; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; import frc.team3128.common.utility.Log; +import frc.team3128.subsystems.Elevator; import frc.team3128.subsystems.Led; import frc.team3128.common.utility.NAR_Shuffleboard; import frc.team3128.subsystems.Swerve; @@ -99,7 +100,11 @@ private void configureButtonBindings() { controller.getButton("Start").onTrue(new InstantCommand(()-> swerve.zeroGyro())); rightStick.getButton(1).onTrue(new InstantCommand(()->swerve.zeroGyro())); - + rightStick.getButton(2).onTrue(new InstantCommand(()-> Elevator.getInstance().set(0.4))).onFalse(new InstantCommand(()-> Elevator.getInstance().set(0))); + rightStick.getButton(3).onTrue(new InstantCommand(()-> Elevator.getInstance().set(-0.4))).onFalse(new InstantCommand(()-> Elevator.getInstance().set(0))); + rightStick.getButton(4).onTrue(moveElevator(30)); + rightStick.getButton(5).onTrue(new InstantCommand(()-> Elevator.getInstance().resetEncoder())); + rightStick.getButton(7).onTrue(Commands.sequence( Commands.deadline(Commands.sequence(new WaitUntilCommand(()-> Math.abs(swerve.getPitch()) > 6), new CmdBangBangBalance()), new CmdBalance()), //new RunCommand(()-> swerve.drive(new Translation2d(CmdBalance.DIRECTION ? -0.25 : 0.25,0),0,true)).withTimeout(0.5), diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index cfcab0a..7bf7891 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -1,14 +1,20 @@ package frc.team3128.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import static edu.wpi.first.wpilibj2.command.Commands.*; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team3128.RobotContainer; import frc.team3128.common.hardware.input.NAR_XboxController; +import frc.team3128.subsystems.Elevator; + public class CmdManager { private static NAR_XboxController controller = RobotContainer.controller; + private static Elevator elevator = Elevator.getInstance(); + private CmdManager() {} @@ -16,4 +22,29 @@ private CmdManager() {} public static CommandBase vibrateController() { return new ScheduleCommand(new WaitCommand(0.5).deadlineWith(new StartEndCommand(() -> controller.startVibrate(), () -> RobotContainer.controller.stopVibrate()))); } + + public static CommandBase moveElevator(double setpoint) { + return sequence( + runOnce(() -> elevator.startPID(setpoint), elevator), + waitUntil(() -> elevator.atSetpoint()) + ); + // return run(() -> elevator.startPID(setpoint), elevator); + } + + public static CommandBase moveElevator(Elevator.States setpoint) { + return sequence( + runOnce(() -> elevator.startPID(setpoint.height), elevator), + waitUntil(() -> elevator.atSetpoint()) + ); + } + + public static CommandBase moveElv(double speed) { + return runOnce(()-> elevator.set(speed), elevator); + } + + + + + + } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Elevator.java b/src/main/java/frc/team3128/subsystems/Elevator.java new file mode 100644 index 0000000..834f36a --- /dev/null +++ b/src/main/java/frc/team3128/subsystems/Elevator.java @@ -0,0 +1,81 @@ +package frc.team3128.subsystems; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; +import static frc.team3128.Constants.ElevatorConstants.*; + +public class Elevator extends NAR_PIDSubsystem { + + private NAR_CANSparkMax m_elv1, m_elv2; + + private static Elevator instance; + + public static Elevator getInstance() { + if (instance == null){ + instance = new Elevator(); + } + return instance; + } + + public enum States { + LOW(0), + MID(0), + HIGH(0), + GROUND(0), + SHELF(0), + DROP(0); + + public double height; + private States(double height) { + this.height = height; + } + } + + public Elevator() { + super(new PIDController(kP, kI, kD), kS, kV, kG); + setConstraints(MIN_DIST, MAX_DIST); + configMotors(); + initShuffleboard(kS, kV, kG); + resetEncoder(); + } + + private void configMotors() { + m_elv1 = new NAR_CANSparkMax(ELV1_ID); + m_elv2 = new NAR_CANSparkMax(ELV2_ID); + + m_elv1.setInverted(false); + m_elv2.setInverted(true); + + m_elv1.setSmartCurrentLimit(CURRENT_LIMIT); + m_elv2.setSmartCurrentLimit(CURRENT_LIMIT); + } + + @Override + protected void useOutput(double output, double setpoint) { + final double power = MathUtil.clamp(output / 12.0, -1, 1); + m_elv1.set(power); + m_elv2.set(power); + } + + public void set(double power) { + disable(); + m_elv1.set(power); + m_elv2.set(power); + } + + public void stop() { + set(0); + } + + public void resetEncoder() { + m_elv1.setSelectedSensorPosition(0); + } + + @Override + public double getMeasurement() { + return m_elv1.getSelectedSensorPosition() / GEAR_RATIO * SPOOL_CIRCUMFERENCE; + } + + +} diff --git a/src/main/java/frc/team3128/subsystems/NAR_PIDSubsystem.java b/src/main/java/frc/team3128/subsystems/NAR_PIDSubsystem.java index c2934e5..3fff57a 100644 --- a/src/main/java/frc/team3128/subsystems/NAR_PIDSubsystem.java +++ b/src/main/java/frc/team3128/subsystems/NAR_PIDSubsystem.java @@ -35,7 +35,7 @@ public abstract class NAR_PIDSubsystem extends SubsystemBase { public NAR_PIDSubsystem(PIDController controller, double kS, double kV, double kG) { m_controller = controller; this.kG_Function = () -> 1; - initShuffleboard(kS, kV, kG); + // initShuffleboard(kS, kV, kG); min = Double.NEGATIVE_INFINITY; max = Double.POSITIVE_INFINITY; } @@ -60,7 +60,7 @@ public void periodic() { } } - private void initShuffleboard(double kS, double kV, double kG) { + public void initShuffleboard(double kS, double kV, double kG) { NAR_Shuffleboard.addComplex(getName(), "PID_Controller", m_controller, 0, 0); NAR_Shuffleboard.addData(getName(), "Enabled", ()-> isEnabled(), 1, 0);