diff --git a/src/main/java/frc/robot/Commands/IntakeCommand.java b/src/main/java/frc/robot/Commands/IntakeCommand.java index ec9c866..fd08b82 100644 --- a/src/main/java/frc/robot/Commands/IntakeCommand.java +++ b/src/main/java/frc/robot/Commands/IntakeCommand.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; -import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.subsystems.IntakeSubsystem; public class IntakeCommand extends CommandBase { diff --git a/src/main/java/frc/robot/Commands/SetElevatorHeightCommand.java b/src/main/java/frc/robot/Commands/SetElevatorHeightCommand.java new file mode 100644 index 0000000..937f259 --- /dev/null +++ b/src/main/java/frc/robot/Commands/SetElevatorHeightCommand.java @@ -0,0 +1,46 @@ +// 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; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ElevatorSubsystem; + +public class SetElevatorHeightCommand extends CommandBase { + + ElevatorSubsystem elevatorSubsystem; + double height; + + /** Creates a new SetElevatorHeightCommand. */ + public SetElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height) { + + this.elevatorSubsystem = elevatorSubsystem; + this.height = height; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(elevatorSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + //set the elevator height + elevatorSubsystem.setHeight(height); + + } + + // 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 elevatorSubsystem.nearTargetHeight(); + } +} diff --git a/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java b/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java index b990610..0059372 100644 --- a/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java +++ b/src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java @@ -5,7 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.subsystems.IntakeSubsystem; public class StopIntakeMotorCommand extends CommandBase { IntakeSubsystem intakeSubsystem; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5261fd..d16543d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,4 +30,22 @@ public static class Intake { public static final double UNLOADING_WAIT_TIME = 2f; } + + public static class Elevator{ + + public static final int RIGHT_ELEVATOR_MOTOR_NUMBER = 14; + public static final int LEFT_ELEVATOR_MOTOR_NUMBER = 15; + + public static final int TICKS_PER_REVOLUTION = 2048; + public static final double GEAR_RATIO = 0.1008d; + public static final double GEAR_CIRCUMFERENCE = 1.432d*Math.PI; + public static final double CARRIAGE_RATIO = 2; + + public static final double MAX_ELEVATOR_EXTENSION = 54; + + public static final double NEAR_TARGET_HEIGHT_THRESHOLD = .5d; + + + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 215ee13..c622363 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.AdvancedIntakeSequence; +import frc.robot.commands.SetElevatorHeightCommand; +import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; /** @@ -22,6 +24,7 @@ public class RobotContainer { public CommandXboxController driverB = new CommandXboxController(0); private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -66,5 +69,8 @@ private void configureButtonBindings() { driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true)); driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false)); driverB.leftBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, false)); + + driverB.a().onTrue(new SetElevatorHeightCommand(elevatorSubsystem, 20)); + driverB.b().onTrue(new SetElevatorHeightCommand(elevatorSubsystem, 40)); } } diff --git a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..a3c40dd --- /dev/null +++ b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java @@ -0,0 +1,119 @@ +// 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; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class ElevatorSubsystem extends SubsystemBase { + /** leader */ + private TalonFX left_motor; + /** follower */ + private TalonFX right_motor; + + private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); + + private double currentHeight; + private double targetHeight; + + private double motorPower; + + private PIDController controller; + + /** Creates a new ElevatorSubsystem. */ + public ElevatorSubsystem() { + + left_motor = new TalonFX(14); + right_motor = new TalonFX(15); + + right_motor.configFactoryDefault(); + left_motor.configFactoryDefault(); + + right_motor.clearStickyFaults(); + left_motor.clearStickyFaults(); + + right_motor.configForwardSoftLimitThreshold( + 0, 0); // this is the bottom limit, we stop AT the bottom + right_motor.configReverseSoftLimitThreshold(-inchesToTicks(Constants.Elevator.MAX_ELEVATOR_EXTENSION), 0); // this is the top limit, we stop at the very top + + right_motor.configForwardSoftLimitEnable(true, 0); + right_motor.configReverseSoftLimitEnable(true, 0); + + right_motor.configOpenloopRamp(.5); + + left_motor.setSelectedSensorPosition(0); + right_motor.setSelectedSensorPosition(0); + + // make sure we hold our height when we get disabled + right_motor.setNeutralMode(NeutralMode.Coast); + left_motor.setNeutralMode(NeutralMode.Coast); + + right_motor.follow(left_motor); + + targetHeight = 0; + + motorPower = 0; + + controller = new PIDController(.02, 0, .01); + + ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower); + + ElevatorTab.addNumber("Left Motor Speed", left_motor::getSelectedSensorVelocity); + ElevatorTab.addNumber("Right Motor Speed", right_motor::getSelectedSensorVelocity); + + ElevatorTab.addNumber("height", () -> this.currentHeight); + ElevatorTab.addNumber("target height", () -> this.targetHeight); + ElevatorTab.addNumber("right motor sensor value", this::getHeight); + + } + + public void setMotorPower(double motorPower){ + this.motorPower = MathUtil.clamp(motorPower, -0.25, 0.25); + } + + public double inchesToTicks(double inches){ + + return (inches / (Constants.Elevator.GEAR_RATIO * Constants.Elevator.GEAR_CIRCUMFERENCE * Constants.Elevator.CARRIAGE_RATIO)) * Constants.Elevator.TICKS_PER_REVOLUTION; + } + + public double ticksToInches(double ticks){ + return (ticks/ Constants.Elevator.TICKS_PER_REVOLUTION) * Constants.Elevator.GEAR_RATIO * Constants.Elevator.GEAR_CIRCUMFERENCE * Constants.Elevator.CARRIAGE_RATIO; + } + + public void setHeight(double targetHeight){ + this.targetHeight = targetHeight; + + controller.setSetpoint(targetHeight); + } + + public double getHeight(){ + return currentHeight; + } + + public boolean nearTargetHeight(){ + return (targetHeight - Constants.Elevator.NEAR_TARGET_HEIGHT_THRESHOLD <= getHeight() + && getHeight() <= targetHeight + Constants.Elevator.NEAR_TARGET_HEIGHT_THRESHOLD); + } + + @Override + public void periodic() { + + currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition()); + + motorPower = controller.calculate(currentHeight); + + + left_motor.set(TalonFXControlMode.PercentOutput, motorPower); + + } +}