Belt Elevator Sample Code

This code controls a belt-driven elevator (like the one shown in the image) that can move up and down. Think of it like a simple lifting mechanism - similar to an elevator in a building, but for a robot. Here's what it does:

1. Safety First:

• Has "limit switches" (like emergency stop buttons) at the top and bottom

• If the elevator hits these switches, it automatically stops to prevent damage

• Limits how much power can go to the motor for safe operation

1. Movement Control:

• Can move up and down smoothly using a motor

• Has precise position control (using something called PID) to move to exact heights

• Can be manually controlled up/down

• Has an emergency stop function

1. Position Tracking:

• Keeps track of where the elevator is using an encoder (like an odometer for the elevator)

• Can tell you the height in inches

• Knows when it's at the top or bottom

1. Information Display:

• Shows important information on the driver's dashboard like:

  • Current height

  • Target height

  • Whether it's at the top or bottom

  • If automatic positioning is turned on

---

BeltElevatorSubsystem.java

package frc.robot.subsystems;

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class BeltElevatorSubsystem extends SubsystemBase {
    private static BeltElevatorSubsystem instance;

    // Motor controller
    private final TalonFX elevatorMotor;
    
    // Limit switches
    private final DigitalInput topLimitSwitch;
    private final DigitalInput bottomLimitSwitch;
    
    // PID Controller
    private final PIDController elevatorPID;
    
    // Constants
    private static final double kP = 0.1;
    private static final double kI = 0.0;
    private static final double kD = 0.0;
    private static final double MAX_VOLTAGE = 10.0;
    
    // State variables
    private double targetPosition = 0.0;
    private boolean isPIDEnabled = true;

    private BeltElevatorSubsystem() {
        // Initialize motor
        elevatorMotor = new TalonFX(Constants.ElevatorConstants.kElevatorMotorID, "rio");
        
        // Configure motor
        TalonFXConfigurator configurator = elevatorMotor.getConfigurator();
        TalonFXConfiguration config = new TalonFXConfiguration();
        config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
        configurator.apply(config);
        
        // Initialize limit switches
        topLimitSwitch = new DigitalInput(Constants.ElevatorConstants.kTopLimitSwitchID);
        bottomLimitSwitch = new DigitalInput(Constants.ElevatorConstants.kBottomLimitSwitchID);
        
        // Initialize PID
        elevatorPID = new PIDController(kP, kI, kD);
        elevatorPID.setTolerance(0.5); // inches
        
        // Reset encoder on initialization
        resetEncoder();
    }

    public static BeltElevatorSubsystem getInstance() {
        if (instance == null) {
            instance = new BeltElevatorSubsystem();
        }
        return instance;
    }

    @Override
    public void periodic() {
        if (isPIDEnabled) {
            double currentPosition = getPosition();
            double output = elevatorPID.calculate(currentPosition, targetPosition);
            
            // Clamp output voltage
            output = Math.min(Math.max(output, -MAX_VOLTAGE), MAX_VOLTAGE);
            
            // Check limit switches before applying voltage
            if ((output > 0 && isAtTopLimit()) || (output < 0 && isAtBottomLimit())) {
                setVoltage(0);
            } else {
                setVoltage(output);
            }
        }
        
        // Update dashboard
        updateTelemetry();
    }

    /**
     * Sets the target height for the elevator in inches
     * @param heightInches desired height in inches
     */
    public void setTargetPosition(double heightInches) {
        targetPosition = heightInches;
    }

    /**
     * Gets the current height of the elevator in inches
     * @return current height in inches
     */
    public double getPosition() {
        // Convert motor rotations to inches using belt reduction and pulley diameter
        return elevatorMotor.getPosition().getValueAsDouble() * Constants.ElevatorConstants.kInchesPerRotation;
    }

    /**
     * Sets the voltage to the elevator motor
     * @param voltage voltage to apply
     */
    public void setVoltage(double voltage) {
        // Check limit switches
        if ((voltage > 0 && isAtTopLimit()) || (voltage < 0 && isAtBottomLimit())) {
            elevatorMotor.setVoltage(0);
            return;
        }
        elevatorMotor.setVoltage(voltage);
    }

    /**
     * Manually move the elevator up
     */
    public void moveUp() {
        if (!isAtTopLimit()) {
            isPIDEnabled = false;
            setVoltage(MAX_VOLTAGE);
        }
    }

    /**
     * Manually move the elevator down
     */
    public void moveDown() {
        if (!isAtBottomLimit()) {
            isPIDEnabled = false;
            setVoltage(-MAX_VOLTAGE);
        }
    }

    /**
     * Stop the elevator
     */
    public void stop() {
        setVoltage(0);
    }

    /**
     * Enable PID control
     */
    public void enablePID() {
        isPIDEnabled = true;
    }

    /**
     * Disable PID control
     */
    public void disablePID() {
        isPIDEnabled = false;
        stop();
    }

    /**
     * Check if elevator is at top limit
     * @return true if top limit switch is pressed
     */
    public boolean isAtTopLimit() {
        return !topLimitSwitch.get(); // Typically limit switches are normally closed
    }

    /**
     * Check if elevator is at bottom limit
     * @return true if bottom limit switch is pressed
     */
    public boolean isAtBottomLimit() {
        return !bottomLimitSwitch.get(); // Typically limit switches are normally closed
    }

    /**
     * Reset the encoder position to zero
     */
    public void resetEncoder() {
        elevatorMotor.setPosition(0);
    }

    /**
     * Update SmartDashboard with relevant values
     */
    private void updateTelemetry() {
        SmartDashboard.putNumber("Elevator/Current Position", getPosition());
        SmartDashboard.putNumber("Elevator/Target Position", targetPosition);
        SmartDashboard.putBoolean("Elevator/At Top Limit", isAtTopLimit());
        SmartDashboard.putBoolean("Elevator/At Bottom Limit", isAtBottomLimit());
        SmartDashboard.putBoolean("Elevator/PID Enabled", isPIDEnabled);
    }
}

---

Robot.java

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.BeltElevatorSubsystem;

public class Robot extends TimedRobot {
    private final XboxController controller = new XboxController(0); // Assumes controller is in port 0
    private final BeltElevatorSubsystem elevator = BeltElevatorSubsystem.getInstance();

    // Preset heights (adjust these based on your needs)
    private static final double GROUND_HEIGHT = 0.0;
    private static final double MIDDLE_HEIGHT = 24.0;
    private static final double HIGH_HEIGHT = 48.0;

    @Override
    public void robotInit() {
        // Any initialization code goes here
    }

    @Override
    public void robotPeriodic() {
        CommandScheduler.getInstance().run();
    }

    @Override
    public void teleopInit() {
        elevator.enablePID(); // Enable PID control when teleop starts
    }

    @Override
    public void teleopPeriodic() {
        // Manual control using left stick Y-axis
        double leftY = -controller.getLeftY(); // Negative because up on stick is negative
        if (Math.abs(leftY) > 0.1) { // Dead zone
            elevator.disablePID(); // Disable PID for manual control
            if (leftY > 0) {
                elevator.moveUp();
            } else {
                elevator.moveDown();
            }
        }

        // Preset heights using buttons
        if (controller.getAButton()) {
            elevator.enablePID();
            elevator.setTargetPosition(GROUND_HEIGHT);
        } else if (controller.getBButton()) {
            elevator.enablePID();
            elevator.setTargetPosition(MIDDLE_HEIGHT);
        } else if (controller.getYButton()) {
            elevator.enablePID();
            elevator.setTargetPosition(HIGH_HEIGHT);
        }

        // Emergency stop with X button
        if (controller.getXButton()) {
            elevator.disablePID();
            elevator.stop();
        }
    }

    @Override
    public void disabledInit() {
        elevator.stop();
    }
}

---

RobotContainer.java

// 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;


import frc.robot.commands.Autos;
import frc.robot.commands.ExampleCommand;
import frc.robot.subsystems.ExampleSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;

/**
 * This class is where the bulk of the robot should be declared. Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
 * subsystems, commands, and trigger mappings) should be declared here.
 */
public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();

  // Replace with CommandPS4Controller or CommandJoystick if needed
  private final CommandXboxController m_driverController =
      new CommandXboxController(1);
      //ADD PORT ABOVE 
      //LISTEN TO THE ABOVE!!

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() {
    // Configure the trigger bindings
    configureBindings();
  }

  /**
   * Use this method to define your trigger->command mappings. Triggers can be created via the
   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
   * predicate, or via the named factories in {@link
   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
   * joysticks}.
   */
  private void configureBindings() {
    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
    new Trigger(m_exampleSubsystem::exampleCondition)
        .onTrue(new ExampleCommand(m_exampleSubsystem));

    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
    // cancelling on release.
    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
  }

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    // An example command will be run in autonomous
    return Autos.exampleAuto(m_exampleSubsystem);
  }
}