Skip to content

Elevator #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
NolanKnight wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Elevator #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
47b46bf
elevator subsystem WIP
NolanKnight Dec 19, 2025
88c9840
elevator in subsystem manager
NolanKnight Jan 9, 2026
577c0a1
robot model
NolanKnight Jan 9, 2026
6021d0a
elevator controls and sim WIP
NolanKnight Jan 9, 2026
0b4fdfd
added manual controls and debugged sim
NolanKnight Jan 10, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 30 additions & 0 deletions assets/Robot_Potato/config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
{
"name": "Potato",
"rotations": [
{
"axis": "x",
"degrees": 90
}
],
"position": [
0,
0,
0
],
"cameras": [],
"components": [
{
"zeroedRotations": [
{
"axis": "x",
"degrees": 90
}
],
"zeroedPositions": [
0,
0,
0
]
}
]
}
Binary file added assets/Robot_Potato/model.glb
View file
Open in desktop
Binary file not shown.
Binary file added assets/Robot_Potato/model_0.glb
View file
Open in desktop
Binary file not shown.
28 changes: 23 additions & 5 deletions src/main/java/com/team973/frc2025/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
package com.team973.frc2025;

import com.team973.frc2025.subsystems.DriveController;
import com.team973.frc2025.subsystems.elevator.ElevatorIO;
import com.team973.lib.devices.GreyPigeonIO;
import com.team973.lib.util.AllianceCache;
import com.team973.lib.util.Joystick;
Expand All @@ -21,29 +22,37 @@ public class Robot extends TimedRobot {

private final GreyPigeonIO m_pigeon = m_subsystemManager.getPigeon();
private final DriveController m_driveController = m_subsystemManager.getDriveController();
private final ElevatorIO m_elevator = m_subsystemManager.getElevator();

private final Joystick m_driverStick =
new Joystick(0, Joystick.Type.XboxController, m_logger.subLogger("driverStick"));
private final Joystick m_coDriverStick =
new Joystick(1, Joystick.Type.XboxController, m_logger.subLogger("coDriverStick"));

private void syncSensors() {
m_driveController.syncSensors();
m_elevator.syncSensors();
}

private void updateSubsystems() {
m_driveController.update();
m_elevator.update();
}

private void resetSubsystems() {
m_driveController.reset();
m_elevator.reset();
}

private void log() {
m_subsystemManager.log();
m_driveController.log();
m_elevator.log();
}

private void updateJoysticks() {
m_driverStick.update();
m_coDriverStick.update();
}

public Robot() {
Expand All @@ -53,6 +62,7 @@ public Robot() {

@Override
public void robotPeriodic() {
syncSensors();
log();
updateJoysticks();
}
Expand All @@ -71,8 +81,6 @@ public void teleopInit() {

@Override
public void teleopPeriodic() {
syncSensors();

double allianceScalar = 1.0;
if (AllianceCache.Get().get() == Alliance.Red) {
// Our gyroscope is blue-centric meaning that facing away from the alliance wall
Expand All @@ -95,16 +103,26 @@ public void teleopPeriodic() {
allianceScalar * m_driverStick.getLeftXAxis() * 0.7,
m_driverStick.getRightXAxis() * 0.8);

if (m_coDriverStick.getAButtonPressed()) {
m_elevator.setTargetPreset(ElevatorIO.Preset.One);
} else if (m_coDriverStick.getBButtonPressed()) {
m_elevator.setTargetPreset(ElevatorIO.Preset.Two);
} else if (m_coDriverStick.getXButtonPressed()) {
m_elevator.setTargetPreset(ElevatorIO.Preset.Three);
} else if (m_coDriverStick.getYButtonPressed()) {
m_elevator.setState(ElevatorIO.State.Manual);
}

m_elevator.setManualInput(m_coDriverStick.getLeftYAxis());

updateSubsystems();
}

@Override
public void disabledInit() {}

@Override
public void disabledPeriodic() {
syncSensors();
}
public void disabledPeriodic() {}

@Override
public void testInit() {}
Expand Down
34 changes: 34 additions & 0 deletions src/main/java/com/team973/frc2025/shared/RobotInfo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import static edu.wpi.first.units.Units.Inches;

import com.team973.lib.util.Conversions;
import com.team973.lib.util.SwerveModuleConfig;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -17,6 +18,39 @@ public class RobotInfo {
public static final String ROBORIO_CANBUS = "";

public static final DriveInfo DRIVE_INFO = new DriveInfo();
public static final ElevatorInfo ELEVATOR_INFO = new ElevatorInfo();

public static class ElevatorInfo {
public final int MOTOR_ID = 20;

public final double MANUAL_INPUT_TO_VOLTS = 12.0;

public final double MOTOR_GEAR_RATIO = 10.0 / 56.0;
public final double MOTOR_ROT_TO_HEIGHT_METERS = MOTOR_GEAR_RATIO * 5.0 * 36.0 * (1.0 / 100.0);

public final double ELEVATOR_KS = 0.0;
public final double ELEVATOR_KV = 0.15;
public final double ELEVATOR_KA = 0.01;
public final double ELEVATOR_KP = 4.0;
public final double ELEVATOR_KI = 0.0;
public final double ELEVATOR_KD = 0.0;

public final double ELEVATOR_MOTION_MAGIC_CRUISE_VELOCITY = 65.0;
public final double ELEVATOR_MOTION_MAGIC_ACCELERATION = 390.0;
public final double ELEVATOR_MOTION_MAGIC_JERK = 2000.0;

public final double ELEVATOR_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.00;

public final double STATOR_CURRENT_LIMIT = 60.0;
public final double SUPPLY_CURRENT_LIMIT = 40.0;

public final double PEAK_FORWARD_VOLTAGE = 12.0;
public final double PEAK_REVERSE_VOLTAGE = -12.0;

public final double PRESET_ONE = 0.0;
public final double PRESET_TWO = 15.0 * Conversions.Distance.METERS_PER_INCH;
public final double PRESET_THREE = 30.0 * Conversions.Distance.METERS_PER_INCH;
}

public static class DriveInfo {
public final int STATUS_SIGNAL_FREQUENCY = 200;
Expand Down
126 changes: 126 additions & 0 deletions src/main/java/com/team973/frc2025/subsystems/elevator/Elevator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,126 @@
package com.team973.frc2025.subsystems.elevator;

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.team973.frc2025.shared.RobotInfo;
import com.team973.lib.devices.GreyTalonFX;
import com.team973.lib.util.Conversions;
import com.team973.lib.util.Logger;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;

public class Elevator extends ElevatorIO {
private final Logger m_logger;

protected final GreyTalonFX m_motor;

private double m_targetPostionHeightMeters;
private double m_manualInput;

public Elevator(Logger logger) {
m_logger = logger;
m_motor =
new GreyTalonFX(
m_elevatorInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight"));

TalonFXConfiguration motorConfig = new TalonFXConfiguration();

motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;

motorConfig.Slot0.kS = m_elevatorInfo.ELEVATOR_KS;
motorConfig.Slot0.kV = m_elevatorInfo.ELEVATOR_KV;
motorConfig.Slot0.kA = m_elevatorInfo.ELEVATOR_KA;
motorConfig.Slot0.kP = m_elevatorInfo.ELEVATOR_KP;
motorConfig.Slot0.kI = m_elevatorInfo.ELEVATOR_KI;
motorConfig.Slot0.kD = m_elevatorInfo.ELEVATOR_KD;

motorConfig.MotionMagic.MotionMagicCruiseVelocity =
m_elevatorInfo.ELEVATOR_MOTION_MAGIC_CRUISE_VELOCITY;
motorConfig.MotionMagic.MotionMagicAcceleration =
m_elevatorInfo.ELEVATOR_MOTION_MAGIC_ACCELERATION;
motorConfig.MotionMagic.MotionMagicJerk = m_elevatorInfo.ELEVATOR_MOTION_MAGIC_JERK;

motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod =
m_elevatorInfo.ELEVATOR_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD;

motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;

motorConfig.CurrentLimits.StatorCurrentLimit = m_elevatorInfo.STATOR_CURRENT_LIMIT;
motorConfig.CurrentLimits.StatorCurrentLimitEnable = true;
motorConfig.CurrentLimits.SupplyCurrentLimit = m_elevatorInfo.SUPPLY_CURRENT_LIMIT;
motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;

motorConfig.Voltage.PeakForwardVoltage = m_elevatorInfo.PEAK_FORWARD_VOLTAGE;
motorConfig.Voltage.PeakReverseVoltage = m_elevatorInfo.PEAK_REVERSE_VOLTAGE;

m_motor.setConfig(motorConfig);
m_motor.setPosition(0.0);

m_targetPostionHeightMeters = Preset.One.getHeightMeters();
m_manualInput = 0.0;
}

@Override
public Pose3d getPose() {
return new Pose3d(
0,
0,
motorRotationsToHeightMeters(m_motor.getPosition().getValueAsDouble())
* Conversions.Distance.METERS_PER_INCH,
new Rotation3d());
}

@Override
public GreyTalonFX getMotor() {
return m_motor;
}

protected double heightMetersToMotorRotations(double postionHeight) {
return postionHeight / m_elevatorInfo.MOTOR_ROT_TO_HEIGHT_METERS;
}

private double motorRotationsToHeightMeters(double motorPostion) {
return motorPostion * m_elevatorInfo.MOTOR_ROT_TO_HEIGHT_METERS;
}

@Override
public void setTargetPreset(Preset preset) {
m_targetPostionHeightMeters = preset.getHeightMeters();
setState(State.ClosedLoop);
}

@Override
public void setManualInput(double input) {
m_manualInput = input;
}

@Override
public double getTargetPositionMotorRot() {
return heightMetersToMotorRotations(m_targetPostionHeightMeters);
}

@Override
public double getManualInput() {
return m_manualInput;
}

@Override
public void syncSensors() {}

@Override
public void log() {
double motorRot = m_motor.getPosition().getValueAsDouble();

m_motor.log();

m_logger.log("currentPostionHeightMeters", motorRotationsToHeightMeters(motorRot));
m_logger.log("targetPostionHeightMeters", m_targetPostionHeightMeters);
m_logger.log("manualInput", m_manualInput);

m_logger.log("state", getState().toString());
}

@Override
public void reset() {}
}
63 changes: 63 additions & 0 deletions src/main/java/com/team973/frc2025/subsystems/elevator/ElevatorIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
package com.team973.frc2025.subsystems.elevator;

import com.team973.frc2025.shared.RobotInfo;
import com.team973.lib.devices.GreyTalonFX;
import com.team973.lib.util.StateMap;
import com.team973.lib.util.Subsystem;
import edu.wpi.first.math.geometry.Pose3d;

public abstract class ElevatorIO extends Subsystem<ElevatorIO.State> {
private final StateMap<State> m_stateMap;

protected static final RobotInfo.ElevatorInfo m_elevatorInfo = RobotInfo.ELEVATOR_INFO;

public enum State {
ClosedLoop,
Manual,
Off
}

public enum Preset {
One(m_elevatorInfo.PRESET_ONE),
Two(m_elevatorInfo.PRESET_TWO),
Three(m_elevatorInfo.PRESET_THREE);

private final double m_heightMeters;

private Preset(double heightMeters) {
m_heightMeters = heightMeters;
}

public double getHeightMeters() {
return m_heightMeters;
}
}

@SuppressWarnings("unchecked")
public ElevatorIO() {
super(State.Off);

m_stateMap =
new StateMap<>(
State.class,
new StateMap.Entry<>(State.ClosedLoop, new ElevatorStates.ClosedLoop(this)),
new StateMap.Entry<>(State.Manual, new ElevatorStates.Manual(this)),
new StateMap.Entry<>(State.Off, new ElevatorStates.Off(this)));
}

public StateMap<State> getStateMap() {
return m_stateMap;
}

public abstract Pose3d getPose();

public abstract GreyTalonFX getMotor();

public abstract void setTargetPreset(Preset preset);

public abstract void setManualInput(double input);

public abstract double getTargetPositionMotorRot();

public abstract double getManualInput();
}
Loading