> For the complete documentation index, see [llms.txt](https://yagsl.gitbook.io/yams/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://yagsl.gitbook.io/yams/documentation/explanation/how-to-set-an-elevators-target-height.md).
# How to set an Elevator's target Height?
`Elevator` setpoints are expressed as `Distance` values (meters, inches, etc.). The motion profile limits how fast the elevator accelerates to the target — so commanding a large height change won't slam the carriage into the top hard stop if your profile parameters are tuned correctly.
### Create and Configure our `Elevator`
Our `Arm` will easily configure the `SmartMotorController` and create a simple and intuitive interface.
// 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 static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
import static edu.wpi.first.units.Units.Feet;
import static edu.wpi.first.units.Units.Pounds;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import yams.mechanisms.SmartMechanism;
import yams.mechanisms.config.ElevatorConfig;
import yams.mechanisms.positional.Elevator;
import yams.motorcontrollers.SmartMotorController;
import yams.motorcontrollers.SmartMotorControllerConfig;
import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode;
import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
import yams.motorcontrollers.local.SparkWrapper;
import yams.gearing.GearBox;
import yams.gearing.MechanismGearing;
public class ExampleSubsystem extends SubsystemBase {
private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this)
.withControlMode(ControlMode.CLOSED_LOOP)
// Drum radius is required for elevators. Chain-driven: specify chain pitch and tooth count.
.withDrumRadius(Inches.of(0.25), 22)
// Feedback Constants (PID Constants)
.withClosedLoopController(4, 0, 0)
.withSimClosedLoopController(4, 0, 0)
// Feedforward Constants
.withFeedforward(new ElevatorFeedforward(0, 0, 0))
.withSimFeedforward(new ElevatorFeedforward(0, 0, 0))
// Telemetry name and verbosity level
.withTelemetry("ElevatorMotor", TelemetryVerbosity.HIGH)
// Gearing from the motor rotor to final shaft.
// In this example gearbox(3,4) is the same as gearbox("3:1","4:1") which corresponds to the gearbox attached to your motor.
.withGearing(new MechanismGearing(GearBox.fromReductionStages(3, 4)))
// Motor properties to prevent over currenting.
.withMotorInverted(false)
.withIdleMode(MotorMode.BRAKE)
.withStatorCurrentLimit(Amps.of(40))
.withClosedLoopRampRate(Seconds.of(0.25))
.withOpenLoopRampRate(Seconds.of(0.25));
// Vendor motor controller object
private SparkMax spark = new SparkMax(4, MotorType.kBrushless);
// Create our SmartMotorController from our Spark and config with the NEO.
private SmartMotorController sparkSmartMotorController = new SparkWrapper(spark, DCMotor.getNEO(1), smcConfig);
private ElevatorConfig elevconfig = new ElevatorConfig()
.withHardLimits(Meters.of(0), Meters.of(3))
.withTelemetry("Elevator", TelemetryVerbosity.HIGH)
.withCarriageWeight(Pounds.of(16));
// Elevator Mechanism
private Elevator elevator = new Elevator(elevconfig, sparkSmartMotorController);
/** Creates a new ExampleSubsystem. */
public ExampleSubsystem() {}
/**
* Example command factory method.
*
* @return a command
*/
public Command exampleMethodCommand() {
// Inline construction of command goes here.
// Subsystem::RunOnce implicitly requires `this` subsystem.
return runOnce(
() -> {
/* one-time action goes here */
});
}
/**
* An example method querying a boolean state of the subsystem (for example, a digital sensor).
*
* @return value of some boolean subsystem state, such as a digital sensor.
*/
public boolean exampleCondition() {
// Query some boolean state, such as a digital sensor.
return false;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
elevator.updateTelemetry();
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
elevator.simIterate();
}
}
### Create `Command`s with our `Elevator`
We use the `Elevator` class as a interface to create commands!
// 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 static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
import static edu.wpi.first.units.Units.Feet;
import static edu.wpi.first.units.Units.Pounds;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import yams.mechanisms.SmartMechanism;
import yams.mechanisms.config.ElevatorConfig;
import yams.mechanisms.positional.Elevator;
import yams.motorcontrollers.SmartMotorController;
import yams.motorcontrollers.SmartMotorControllerConfig;
import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode;
import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
import yams.motorcontrollers.local.SparkWrapper;
import yams.gearing.GearBox;
import yams.gearing.MechanismGearing;
public class ExampleSubsystem extends SubsystemBase {
private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this)
.withControlMode(ControlMode.CLOSED_LOOP)
// Drum radius is required for elevators. Chain-driven: specify chain pitch and tooth count.
.withDrumRadius(Inches.of(0.25), 22)
// Feedback Constants (PID Constants)
.withClosedLoopController(4, 0, 0, MetersPerSecond.of(0.5), MetersPerSecondPerSecond.of(0.5))
.withSimClosedLoopController(4, 0, 0, MetersPerSecond.of(0.5), MetersPerSecondPerSecond.of(0.5))
// Feedforward Constants
.withFeedforward(new ElevatorFeedforward(0, 0, 0))
.withSimFeedforward(new ElevatorFeedforward(0, 0, 0))
// Telemetry name and verbosity level
.withTelemetry("ElevatorMotor", TelemetryVerbosity.HIGH)
// Gearing from the motor rotor to final shaft.
// In this example gearbox(3,4) is the same as gearbox("3:1","4:1") which corresponds to the gearbox attached to your motor.
.withGearing(new MechanismGearing(GearBox.fromReductionStages(3, 4)))
// Motor properties to prevent over currenting.
.withMotorInverted(false)
.withIdleMode(MotorMode.BRAKE)
.withStatorCurrentLimit(Amps.of(40))
.withClosedLoopRampRate(Seconds.of(0.25))
.withOpenLoopRampRate(Seconds.of(0.25));
// Vendor motor controller object
private SparkMax spark = new SparkMax(4, MotorType.kBrushless);
// Create our SmartMotorController from our Spark and config with the NEO.
private SmartMotorController sparkSmartMotorController = new SparkWrapper(spark, DCMotor.getNEO(1), smcConfig);
private ElevatorConfig elevconfig = new ElevatorConfig()
.withHardLimits(Meters.of(0), Meters.of(3))
.withTelemetry("Elevator", TelemetryVerbosity.HIGH)
.withMechanismPositionConfig(m_robotToMechanism)
.withCarriageWeight(Pounds.of(16));
// Elevator Mechanism
private Elevator elevator = new Elevator(elevconfig, sparkSmartMotorController);
/**
* Set the height of the elevator.
* @param angle Distance to go to.
*/
public Command run(Distance height) { return elevator.run(height);}
/**
* Move the elevator up and down.
* @param dutycycle [-1, 1] speed to set the elevator too.
*/
public Command set(double dutycycle) { return elevator.set(dutycycle);}
/** Creates a new ExampleSubsystem. */
public ExampleSubsystem() {}
@Override
public void periodic() {
// This method will be called once per scheduler run
elevator.updateTelemetry();
}
@Override
public void simulationPeriodic() {
// This method will be called once per scheduler run during simulation
elevator.simIterate();
}
}
### Bind buttons to our `Elevator`
We bind buttons to use the `Commands` from our `Elevator`
// 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.Constants.OperatorConstants;
import frc.robot.commands.Autos;
import frc.robot.commands.ExampleCommand;
import frc.robot.subsystems.ExampleSubsystem;
import static edu.wpi.first.units.Units.Meters;
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(OperatorConstants.kDriverControllerPort);
/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
// Set the default command to force the elevator to go to 0.
m_exampleSubsystem.setDefaultCommand(m_exampleSubsystem.run(Meters.of(0)));
}
/**
* 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 `run` when the Xbox controller's B button is pressed,
// cancelling on release.
m_driverController.a().whileTrue(m_exampleSubsystem.run(Meters.of(0.5)));
m_driverController.b().whileTrue(m_exampleSubsystem.run(Meters.of(1)));
// Schedule `set` when the Xbox controller's B button is pressed,
// cancelling on release.
m_driverController.x().whileTrue(m_exampleSubsystem.set(0.3));
m_driverController.y().whileTrue(m_exampleSubsystem.set(-0.3));
}
/**
* 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);
}
}
### Simulate our Elevator!
We can use our `Elevator` in simulation, with the exact same code that will control the real robot!
Connect an Xbox controller to your system and drag and drop from **System Joysticks** to **Joystick\[0]**
Open up the Simulated mechanism with **NetworkTables -> SmartDashboard -> Arm -> mechanism**
Resize **Elevator/mechanism** to your liking
Press **Teleoperated** in **Robot State** then you can use your controller like its controlling the real robot!
Congratulations on successfully programming your Elevator!! :tada::tada: