Climber

src/main/java/frc/robot/Constants.java

@@ -17,6 +17,17 @@
  */
 public final class Constants {
 
+	public static final double kElevatorP = 0;
+	public static final double kElevatorI = 0;
+    public static final double kElevatorD = 0;
+    public static final int kElevatorHoldVoltage = 0;
+    public static final double kClimbThreshold = 100;
+
+    public static final double kClimbHeight = 0;
+    public static final double kClimbZero = 0;
+    public static final double kLevelP = 0;
+	public static final int kClimbTicks = 0;
+
     public static double kFlywheelP = 0.07;
     public static double kFlywheelD = 0;
     public static double kFlywheelFF = .0465;

src/main/java/frc/robot/Hardware.java

@@ -8,15 +8,17 @@
 package frc.robot;
 
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.kauailabs.navx.frc.AHRS;
-
 import frc.robot.util.Limelight;
 
 /**
  * Add your docs here.
  */
 public class Hardware {
-
+    public static TalonFX elevatorMaster;
+    public static TalonSRX levelMotor;
+
     public static TalonFX flywheelMaster;
     public static TalonFX flywheelFollower;
 

src/main/java/frc/robot/OI.java

@@ -9,6 +9,9 @@
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.ClimbAndLevelCommand;
+import frc.robot.commands.RaiseElevatorCommand;
+import frc.robot.commands.ZeroElevator;
 import frc.robot.commands.SetFlywheelRPM;
 import frc.robot.commands.AutoAlign;
 import frc.robot.commands.DriveWithJoystick;
@@ -19,6 +22,24 @@
  */
 public class OI {
 
+
+    private Joystick operatorJoystick;
+    private JoystickButton raiseElevatorButton;
+    private JoystickButton climbButton;
+    private JoystickButton zeroElevatorButton;
+
+    public OI(){
+        operatorJoystick = new Joystick(1);
+
+        raiseElevatorButton = new JoystickButton(operatorJoystick, 1);
+        climbButton = new JoystickButton(operatorJoystick, 2);
+        zeroElevatorButton = new JoystickButton(operatorJoystick, 3);
+
+        raiseElevatorButton.whenPressed(new RaiseElevatorCommand());
+        climbButton.whenPressed(new ClimbAndLevelCommand());
+        zeroElevatorButton.whenActive(new ZeroElevator());
+    }
+
     private Joystick driverJoystick;
     private JoystickButton shootBall;
     private JoystickButton stopFlywheel;
@@ -27,6 +48,11 @@ public class OI {
     private JoystickButton align;
     private JoystickButton quickTurnButton;
 
+    private JoystickButton raiseElevatorButton;
+    private JoystickButton climbButton;
+    private JoystickButton zeroElevatorButton;
+
+
     public OI(){
         driverJoystick = new Joystick(0);
         shootBall = new JoystickButton(driverJoystick, 1);
@@ -41,6 +67,14 @@ public OI(){
 
         shootBall.whenPressed(new SetFlywheelRPM(3250));
         stopFlywheel.whenPressed(new SetFlywheelRPM(0));
+
+        raiseElevatorButton = new JoystickButton(operatorJoystick, 1);
+        climbButton = new JoystickButton(operatorJoystick, 2);
+        zeroElevatorButton = new JoystickButton(operatorJoystick, 3);
+
+        raiseElevatorButton.whenPressed(new RaiseElevatorCommand());
+        climbButton.whenPressed(new ClimbAndLevelCommand());
+        zeroElevatorButton.whenActive(new ZeroElevator());
     }
 
     public double getWheel()

src/main/java/frc/robot/Robot.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Flywheel;
 
 import java.util.ArrayList;
@@ -49,6 +50,10 @@ public class Robot extends TimedRobot {
 
   //private RobotContainer m_robotContainer;
 
+
+  public static Climber climber;
+  public static OI oi;
+
   public enum RobotState{
     DISABLED, TELEOP, AUTON
   };
@@ -72,6 +77,8 @@ public void robotInit() {
 
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
+    climber = new Climber();
+    oi = new OI();
     //m_robotContainer = new RobotContainer();
   }
 
@@ -114,8 +121,6 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    //m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
     drivetrain.setPathDirection(false);
     Trajectory traj1 = TrajectoryGenerator.generateTrajectory(List.of(
       new Pose2d(),

src/main/java/frc/robot/commands/ClimbAndLevelCommand.java

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Robot;
+import frc.robot.subsystems.Climber.ElevatorState;
+
+public class ClimbAndLevelCommand extends CommandBase {
+  /**
+   * Creates a new ClimbAndLevelCommand.
+   */
+  public ClimbAndLevelCommand() {
+    addRequirements(Robot.climber);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    Robot.climber.setElevatorState(ElevatorState.PULLING);
+  }
+
+  // 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 true;
+  }
+}

src/main/java/frc/robot/commands/RaiseElevatorCommand.java

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Robot;
+import frc.robot.subsystems.Climber.ElevatorState;
+
+public class RaiseElevatorCommand extends CommandBase {
+  /**
+   * Creates a new RaiseElevator.
+   */
+  public RaiseElevatorCommand() {
+    addRequirements(Robot.climber);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    Robot.climber.setElevatorState(ElevatorState.SETPOINT);
+  }
+
+  // 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 true;
+  }
+}

src/main/java/frc/robot/commands/ZeroElevator.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Robot;
+import frc.robot.subsystems.Climber.ElevatorState;
+
+public class ZeroElevator extends CommandBase {
+  /**
+   * Creates a new ZeroElevator.
+   */
+  public ZeroElevator() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    Robot.climber.setElevatorState(ElevatorState.ZEROING);
+  }
+
+  // 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;
+  }
+}

src/main/java/frc/robot/subsystems/Climber.java

@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.Hardware;
+
+public class Climber extends SubsystemBase {
+  /**
+   * Creates a new Elevator.
+   */
+
+  private ElevatorState currState;
+  private double angleError;
+
+  public enum ElevatorState {
+    PULLING, SETPOINT, HOLD, ZEROED, LEVEL, ZEROING
+  };
+
+  public Climber() {
+
+    Hardware.elevatorMaster = new TalonFX(0);
+    Hardware.levelMotor = new TalonSRX(1);
+
+    Hardware.elevatorMaster.configFactoryDefault();
+    Hardware.levelMotor.configFactoryDefault();
+
+    Hardware.elevatorMaster.setInverted(false);
+    Hardware.levelMotor.setInverted(false);
+
+    Hardware.elevatorMaster.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDIdx,
+        Constants.kTimeoutMs);
+
+    Hardware.elevatorMaster.setSelectedSensorPosition(0);
+
+    currState = ElevatorState.ZEROED;
+
+    Hardware.elevatorMaster.configPeakOutputForward(1);
+    Hardware.elevatorMaster.configPeakOutputReverse(-1);
+
+    Hardware.elevatorMaster.config_kP(Constants.kPIDIdx, Constants.kElevatorP);
+    Hardware.elevatorMaster.config_kI(Constants.kPIDIdx, Constants.kElevatorI);
+    Hardware.elevatorMaster.config_kD(Constants.kPIDIdx, Constants.kElevatorD);
+
+  }
+
+  public void periodic() {
+
+    switch (currState) {
+
+    case SETPOINT:
+      Hardware.elevatorMaster.set(ControlMode.Position, Constants.kClimbHeight, DemandType.ArbitraryFeedForward, Constants.kElevatorHoldVoltage/12);
+      if(Math.abs(Hardware.elevatorMaster.getSelectedSensorPosition() - Constants.kClimbTicks) < Constants.kClimbThreshold){
+        currState = ElevatorState.HOLD;
+      }
+      break;
+
+    case HOLD:
+      break;
+
+    case PULLING:
+      Hardware.elevatorMaster.set(ControlMode.Position, Constants.kClimbZero, DemandType.ArbitraryFeedForward, Constants.kElevatorHoldVoltage/12);
+      if(Math.abs(Hardware.elevatorMaster.getSelectedSensorPosition() - Constants.kClimbZero) < Constants.kClimbThreshold){
+        currState = ElevatorState.LEVEL;
+      }
+      break;
+
+    case LEVEL:
+      Hardware.levelMotor.set(ControlMode.PercentOutput, angleError * Constants.kLevelP);
+      break;
+
+    case ZEROING:
+      Hardware.elevatorMaster.set(ControlMode.Position, Constants.kClimbZero, DemandType.ArbitraryFeedForward, Constants.kElevatorHoldVoltage/12);
+      if(Math.abs(Hardware.elevatorMaster.getSelectedSensorPosition() - Constants.kClimbZero) < Constants.kClimbThreshold){
+        currState = ElevatorState.ZEROED;
+      }
+      break;
+
+    case ZEROED:
+      stop();
+      break;
+    }
+  }
+
+  public void stop() {
+    Hardware.elevatorMaster.set(ControlMode.PercentOutput, 0);
+  }
+
+  public void setElevatorState(ElevatorState desiredState){
+    currState = desiredState;
+  }
+
+  public ElevatorState getElevatorState(){
+    return currState;
+  }
+}

vendordeps/Phoenix.json

@@ -153,6 +153,7 @@
             "groupId": "com.ctre.phoenix",
             "artifactId": "platform-stub",
             "version": "5.17.6",
+
             "libName": "CTRE_PhoenixPlatform",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -166,6 +167,7 @@
             "groupId": "com.ctre.phoenix",
             "artifactId": "core",
             "version": "5.17.6",
+
             "libName": "CTRE_PhoenixCore",
             "headerClassifier": "headers",
             "sharedLibrary": false,