From 3503acc53157d7787bb017825482749e2df096cc Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Tue, 28 Jan 2025 18:28:40 -0500 Subject: [PATCH 1/7] implemented elevator and commands Co-authored-by: simon Co-authored-by: Haiiden Co-authored-by: qingque6 Co-authored-by: BrianZ60 Co-authored-by: Ardian Agoes --- .../commands/elevator/ElevatorToBottom.java | 9 + .../commands/elevator/ElevatorToHandoff.java | 9 + .../commands/elevator/ElevatorToHeight.java | 21 ++ .../commands/elevator/ElevatorToLvl2.java | 9 + .../commands/elevator/ElevatorToLvl3.java | 9 + .../commands/elevator/ElevatorToLvl4.java | 8 + .../commands/elevator/ElevatorToTop.java | 10 + .../stuypulse/robot/constants/Constants.java | 22 +++ .../com/stuypulse/robot/constants/Ports.java | 6 + .../stuypulse/robot/constants/Settings.java | 37 +++- .../robot/subsystems/elevator/Elevator.java | 39 ++++ .../subsystems/elevator/ElevatorImpl.java | 101 ++++++++++ .../subsystems/elevator/ElevatorSimu.java | 91 +++++++++ .../elevator/ElevatorVisualizer.java | 187 ++++++++++++++++++ 14 files changed, 557 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java create mode 100644 src/main/java/com/stuypulse/robot/constants/Constants.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java new file mode 100644 index 0000000..bc6629b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Constants.Elevator; + +public class ElevatorToBottom extends ElevatorToHeight{ + public ElevatorToBottom(){ + super(Elevator.MIN_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java new file mode 100644 index 0000000..c8e6f39 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToHandoff extends ElevatorToHeight{ + public ElevatorToHandoff() { + super(Elevator.HANDOFF_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java new file mode 100644 index 0000000..f8a6da0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.subsystems.elevator.Elevator; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ElevatorToHeight extends InstantCommand { + private final Elevator elevator; + private final double targetHeight; + + public ElevatorToHeight(double targetHeight){ + elevator = Elevator.getInstance(); + this.targetHeight = targetHeight; + + addRequirements(elevator); + } + + public void initialize(){ + elevator.setTargetHeight(targetHeight); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java new file mode 100644 index 0000000..1d861e6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2 extends ElevatorToHeight{ + public ElevatorToLvl2(){ + super(Elevator.L2_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java new file mode 100644 index 0000000..ff4a056 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3 extends ElevatorToHeight{ + public ElevatorToLvl3(){ + super(Elevator.L3_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java new file mode 100644 index 0000000..d674636 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java @@ -0,0 +1,8 @@ +package com.stuypulse.robot.commands.elevator; +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4 extends ElevatorToHeight{ + public ElevatorToLvl4() { + super(Elevator.L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java new file mode 100644 index 0000000..bba0cb4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Constants; + +public class ElevatorToTop extends ElevatorToHeight{ + public ElevatorToTop() { + super(Constants.Elevator.MAX_HEIGHT_METERS); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java new file mode 100644 index 0000000..e9b30a1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.constants; + +import edu.wpi.first.math.util.Units; + +public class Constants { + public interface Elevator { + double MIN_HEIGHT_METERS = Units.inchesToMeters(9.09375); // FROM THE BOTTOM OF FIXED STAGE TO TOP OF CARRIAGE + double MAX_HEIGHT_METERS = Units.inchesToMeters(77); // FROM THE BOTTOM OF FIXED STAGE TO TOP ELEVATOR + + double MASS_KG = 10.0; + double DRUM_RADIUS_METERS = (MAX_HEIGHT_METERS / Encoders.NUM_ROTATIONS_TO_REACH_TOP * Encoders.GEARING) / 2 / Math.PI; + + public interface Encoders { + double GEARING = 4.0; + + double NUM_ROTATIONS_TO_REACH_TOP = (6 + 9.0 / 24) * GEARING; // Number of rotations that the motor has to spin, NOT the gear + + double POSITION_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP; + double VELOCITY_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP / 60; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..9ef9a4e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,10 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } + + public interface Elevator { + int MOTOR = 0; + int BOTTOM_SWITCH = 1; + int TOP_SWITCH = 2; + } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..e6bf627 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -14,4 +14,39 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + + double DT = 0.020; + + public interface Elevator { + SmartNumber MAX_VELOCITY_METERS_PER_SECOND = new SmartNumber("Elevator/Max Velocity (m per s)", 1.0); + SmartNumber MAX_ACCEL_METERS_PER_SECOND_PER_SECOND = new SmartNumber("Elevator/Max Accel (m per s^2)", 2.0); + + // CHANGE + double HANDOFF_HEIGHT_METERS = 0.1; + double L2_HEIGHT_METERS = 0.25; + double L3_HEIGHT_METERS = 0.5; + double L4_HEIGHT_METERS = 0.75; + + double FEED_HEIGHT_METERS = 0.4; + + SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); + + public interface PID { + SmartNumber kP = new SmartNumber("Elevator/Controller/kP",10); + SmartNumber kI = new SmartNumber("Elevator/Controller/kI",0.0); + SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.2); + } + + public interface FF { + SmartNumber kS = new SmartNumber("Elevator/Controller/kS",0.20506); + SmartNumber kV = new SmartNumber("Elevator/Controller/kV",3.7672); + SmartNumber kA = new SmartNumber("Elevator/Controller/kA", 0.27); + SmartNumber kG = new SmartNumber("Elevator/Controller/kG", 1.37); + } + + public interface Simulation { + double SCALE_FACTOR = 0.5 + 2.5/77; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..a0e1195 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,39 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.stuypulse.robot.Robot; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Elevator extends SubsystemBase { + + private static final Elevator instance; + + static { + if (Robot.isReal()) { + instance = new ElevatorImpl(); + } else { + instance = new ElevatorSimu(); + } + } + + private final ElevatorVisualizer visualizer; + + public static Elevator getInstance() { + return instance; + } + + public Elevator() { + visualizer = ElevatorVisualizer.getInstance(); + } + + public abstract void setTargetHeight(double height); + public abstract double getTargetHeight(); + public abstract double getCurrentHeight(); + public abstract boolean atTargetHeight(); + + public void periodic() { + visualizer.update(); + } + +} + \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java new file mode 100644 index 0000000..3e5f4ba --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -0,0 +1,101 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorImpl extends Elevator { + private final TalonFX motor; + + private final DigitalInput bottomBumpSwitch; + private final DigitalInput topBumpSwitch; + + private final SmartNumber targetHeight; + + private final Controller controller; + + private boolean hasBeenReset; + + public ElevatorImpl() { + motor = new TalonFX(Ports.Elevator.MOTOR); + + bottomBumpSwitch = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); + topBumpSwitch = new DigitalInput(Ports.Elevator.TOP_SWITCH); + + targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); + + MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + + controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() + .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) + .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) + .setSetpointFilter(motionProfile); + + hasBeenReset = false; + } + + @Override + public void setTargetHeight(double height) { + targetHeight.set( + SLMath.clamp( + height, + Constants.Elevator.MIN_HEIGHT_METERS, + Constants.Elevator.MAX_HEIGHT_METERS + ) + ); + } + + @Override + public double getTargetHeight() { + return targetHeight.getAsDouble(); + } + + @Override + public double getCurrentHeight() { + return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR; + } + + @Override + public boolean atTargetHeight() { + return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); + } + + private void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + @Override + public void periodic() { + super.periodic(); + + if (bottomBumpSwitch.get()) { + hasBeenReset = true; + motor.setPosition(0); + } + + if (topBumpSwitch.get()) { + hasBeenReset = true; + motor.setPosition(Constants.Elevator.MAX_HEIGHT_METERS / Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); + } + + if (!hasBeenReset) { + setVoltage(-1); + } else { + controller.update(getTargetHeight(), getCurrentHeight()); + setVoltage(controller.getOutput()); + } + + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java new file mode 100644 index 0000000..1b54558 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -0,0 +1,91 @@ +package com.stuypulse.robot.subsystems.elevator; + +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorSimu extends Elevator { + + private final ElevatorSim sim; + private final double minHeight, maxHeight; + + private final SmartNumber targetHeight; + + private final Controller controller; + + ElevatorSimu() { + + sim = new ElevatorSim( + DCMotor.getNEO(2), + Constants.Elevator.Encoders.GEARING, + Constants.Elevator.MASS_KG, + Constants.Elevator.DRUM_RADIUS_METERS, + Constants.Elevator.MIN_HEIGHT_METERS, + Constants.Elevator.MAX_HEIGHT_METERS, + true, + Constants.Elevator.MIN_HEIGHT_METERS + ); + + minHeight = Constants.Elevator.MIN_HEIGHT_METERS; + maxHeight = Constants.Elevator.MAX_HEIGHT_METERS; + + targetHeight = new SmartNumber("Elevator/Target Height (m)", Constants.Elevator.MIN_HEIGHT_METERS); + + MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + + controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() + .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) + .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) + .setSetpointFilter(motionProfile); + } + + public ElevatorSim getSim() { + return sim; + } + + @Override + public void setTargetHeight(double height) { + targetHeight.set(SLMath.clamp(height, minHeight, maxHeight)); + } + + @Override + public double getTargetHeight() { + return targetHeight.get(); + } + + @Override + public double getCurrentHeight() { + return sim.getPositionMeters(); + } + + @Override + public boolean atTargetHeight() { + return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); + } + + @Override + public void periodic() { + super.periodic(); + + controller.update(getTargetHeight(), getCurrentHeight()); + sim.setInputVoltage(controller.getOutput()); + sim.update(Settings.DT); + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + + ElevatorVisualizer.getInstance().update(); + + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java new file mode 100644 index 0000000..623ee97 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -0,0 +1,187 @@ +package com.stuypulse.robot.subsystems.elevator; + +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ElevatorVisualizer { + + private static final ElevatorVisualizer instance; + + static { + instance = new ElevatorVisualizer(); + } + + private final Mechanism2d elevator2d; + + private final MechanismRoot2d elevatorBL; + private final MechanismRoot2d elevatorTR; + + private final MechanismRoot2d outerBL; + private final MechanismRoot2d outerTR; + + private final MechanismRoot2d innerBL; + private final MechanismRoot2d innerTR; + + public static ElevatorVisualizer getInstance(){ + return instance; + } + + public ElevatorVisualizer() { + + // Mechanism2d + elevator2d = new Mechanism2d(Units.inchesToMeters(17), Units.inchesToMeters(150)); + + // Stage One + // Bottom Left Node + elevatorBL = elevator2d.getRoot("Elevator BL", Units.inchesToMeters(2), Constants.Elevator.MIN_HEIGHT_METERS); + + elevatorBL.append(new MechanismLigament2d( + "Left Tower", + Units.inchesToMeters(47), + 90, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + elevatorBL.append(new MechanismLigament2d( + "Bottom Tower", + Units.inchesToMeters(11), + 0, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + // Top Right Node + elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(13), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + elevatorTR.append(new MechanismLigament2d( + "Right Tower", + Units.inchesToMeters(47), + -90, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + elevatorTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(11), + 180, + 10, + new Color8Bit(Color.kOrange) + ) + ); + + // Stage Two + // Bottom Left Node + outerBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); + + outerBL.append(new MechanismLigament2d( + "Left Side", + Units.inchesToMeters(47), + 90, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + outerBL.append(new MechanismLigament2d( + "Bottom Side", + Units.inchesToMeters(9), + 0, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + // Top Right Node + outerTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + outerTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(9), + 180, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + outerTR.append(new MechanismLigament2d( + "Right Side", + Units.inchesToMeters(47), + -90, + 10, + new Color8Bit(Color.kYellow) + ) + ); + + // Carriage + // Bottom Left Node + innerBL = elevator2d.getRoot("Inner BL", Units.inchesToMeters(4), Units.inchesToMeters(1) + Constants.Elevator.MIN_HEIGHT_METERS); + + innerBL.append(new MechanismLigament2d( + "Left Side", + Units.inchesToMeters(7), + 90, + 10, + new Color8Bit(Color.kPink) + ) + ); + + innerBL.append(new MechanismLigament2d( + "Bottom Side", + Units.inchesToMeters(7), + 0, + 10, + new Color8Bit(Color.kPink) + ) + ); + + // Top Right Node + innerTR = elevator2d.getRoot("Inner TR", Units.inchesToMeters(11), Units.inchesToMeters(8) + Constants.Elevator.MIN_HEIGHT_METERS); + + innerTR.append(new MechanismLigament2d( + "Top Side", + Units.inchesToMeters(7), + 180, + 10, + new Color8Bit(Color.kPink) + ) + ); + + innerTR.append(new MechanismLigament2d( + "Right Side", + Units.inchesToMeters(7), + -90, + 10, + new Color8Bit(Color.kPink) + ) + ); + + SmartDashboard.putData("Visualizers/Elevator", elevator2d); + } + + public void update() { + // Top of Carriage is Target Height + Elevator elevator = Elevator.getInstance(); + + outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Constants.Elevator.MIN_HEIGHT_METERS); + outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + + innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); + innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + + } + +} \ No newline at end of file From a12406ff93969d79b2cac6639e5a5d366610a3a6 Mon Sep 17 00:00:00 2001 From: Baiulus Date: Fri, 31 Jan 2025 18:51:33 -0500 Subject: [PATCH 2/7] implemented motion magic, configured motors, used bstream, added logging, and changed visualizer Co-authored-by: simon Co-authored-by: EvanK-h Co-authored-by: Ardian Agoes Co-authored-by: qingque6 Co-authored-by: BrianZ60 Co-authored-by: Matthew Cheng Co-authored-by: Zixi Qiao Co-authored-by: ryanyu32 Co-authored-by: shiva4503 --- networktables.json | 1 + simgui-ds.json | 92 ++++++++++++++++++ simgui.json | 29 ++++++ .../com/stuypulse/robot/RobotContainer.java | 2 + .../commands/elevator/ElevatorToHeight.java | 12 ++- .../stuypulse/robot/constants/Settings.java | 11 +++ .../robot/subsystems/elevator/Elevator.java | 2 + .../subsystems/elevator/ElevatorImpl.java | 94 +++++++++++-------- .../subsystems/elevator/ElevatorSimu.java | 13 ++- .../elevator/ElevatorVisualizer.java | 15 ++- 10 files changed, 223 insertions(+), 48 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..d705b1b --- /dev/null +++ b/simgui.json @@ -0,0 +1,29 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Visualizers/Elevator": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/Visualizers/Elevator": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Elevator": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..49fe7f1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.robot.subsystems.elevator.Elevator; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,6 +22,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Elevator elevator = Elevator.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java index f8a6da0..28baf31 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -1,21 +1,31 @@ package com.stuypulse.robot.commands.elevator; +import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.subsystems.elevator.Elevator; +import com.stuypulse.stuylib.streams.booleans.BStream; import edu.wpi.first.wpilibj2.command.InstantCommand; public class ElevatorToHeight extends InstantCommand { private final Elevator elevator; private final double targetHeight; + private final BStream belowTop, aboveBottom; public ElevatorToHeight(double targetHeight){ elevator = Elevator.getInstance(); this.targetHeight = targetHeight; + belowTop = BStream.create(() -> !elevator.atTop()) + .and(() -> targetHeight < Constants.Elevator.MAX_HEIGHT_METERS); + aboveBottom = BStream.create(() -> !elevator.atBottom()) + .and(() -> targetHeight > Constants.Elevator.MIN_HEIGHT_METERS); + addRequirements(elevator); } public void initialize(){ - elevator.setTargetHeight(targetHeight); + if (belowTop.get() && aboveBottom.get()) { + elevator.setTargetHeight(targetHeight); + } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e6bf627..d36719b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -30,6 +30,17 @@ public interface Elevator { double FEED_HEIGHT_METERS = 0.4; + double RAMP_RATE = 0.1; + + // FIND OUT REAL GEAR RATIO + double GEAR_RATIO = 1.0/5.0; + double CURRENT_LIMIT = 5.0; + + // Magic motion, change RPS + double TARGET_CRUISE_VELOCITY = 0.0; //Rotations Per Second + double TARGET_ACCELERATION = 0.0; //Rotations Per Second^2 + double TARGET_JERK = 0.0; //Rotations Per Second^3 + SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); public interface PID { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index a0e1195..52864cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -30,6 +30,8 @@ public Elevator() { public abstract double getTargetHeight(); public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); + public abstract boolean atTop(); + public abstract boolean atBottom(); public void periodic() { visualizer.update(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 3e5f4ba..787838e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -1,48 +1,65 @@ package com.stuypulse.robot.subsystems.elevator; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward; -import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; -import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ElevatorImpl extends Elevator { private final TalonFX motor; - - private final DigitalInput bottomBumpSwitch; - private final DigitalInput topBumpSwitch; - private final SmartNumber targetHeight; - - private final Controller controller; - - private boolean hasBeenReset; + private final DigitalInput bumpSwitchTop, bumpSwitchBottom; public ElevatorImpl() { motor = new TalonFX(Ports.Elevator.MOTOR); - - bottomBumpSwitch = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); - topBumpSwitch = new DigitalInput(Ports.Elevator.TOP_SWITCH); - targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); - MotionProfile motionProfile = new MotionProfile(Settings.Elevator.MAX_VELOCITY_METERS_PER_SECOND, Settings.Elevator.MAX_ACCEL_METERS_PER_SECOND_PER_SECOND); + bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); + bumpSwitchTop = new DigitalInput(Ports.Elevator.TOP_SWITCH); - controller = new MotorFeedforward(Settings.Elevator.FF.kS, Settings.Elevator.FF.kV, Settings.Elevator.FF.kA).position() - .add(new ElevatorFeedforward(Settings.Elevator.FF.kG)) - .add(new PIDController(Settings.Elevator.PID.kP, Settings.Elevator.PID.kI, Settings.Elevator.PID.kD)) - .setSetpointFilter(motionProfile); + TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); + + Slot0Configs slot0 = new Slot0Configs(); + + slot0.kS = Settings.Elevator.FF.kS.getAsDouble(); + slot0.kV = Settings.Elevator.FF.kV.getAsDouble(); + slot0.kA = Settings.Elevator.FF.kA.getAsDouble(); + slot0.kG = Settings.Elevator.FF.kG.getAsDouble(); + slot0.kP = Settings.Elevator.PID.kP.getAsDouble(); + slot0.kI = Settings.Elevator.PID.kI.getAsDouble(); + slot0.kD = Settings.Elevator.PID.kD.getAsDouble(); + + elevatorMotorConfig.Slot0 = slot0; - hasBeenReset = false; + // Base motor configs + elevatorMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + elevatorMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + elevatorMotorConfig.Feedback.SensorToMechanismRatio = Settings.Elevator.GEAR_RATIO; + + // Current limiting + elevatorMotorConfig.CurrentLimits.StatorCurrentLimit = Settings.Elevator.CURRENT_LIMIT; + elevatorMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + // Ramp motor voltage + elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE; + elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE; + + var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; + elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; + elevatorMotionMagicConfigs.MotionMagicAcceleration = Settings.Elevator.TARGET_ACCELERATION; + elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; + + motor.getConfigurator().apply(elevatorMotorConfig); } @Override @@ -71,31 +88,26 @@ public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); } - private void setVoltage(double voltage) { - motor.setVoltage(voltage); + @Override + public boolean atBottom() { + return !bumpSwitchBottom.get(); + } + + @Override + public boolean atTop() { + return !bumpSwitchTop.get(); } @Override public void periodic() { super.periodic(); - if (bottomBumpSwitch.get()) { - hasBeenReset = true; - motor.setPosition(0); - } - - if (topBumpSwitch.get()) { - hasBeenReset = true; - motor.setPosition(Constants.Elevator.MAX_HEIGHT_METERS / Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); - } - - if (!hasBeenReset) { - setVoltage(-1); - } else { - controller.update(getTargetHeight(), getCurrentHeight()); - setVoltage(controller.getOutput()); - } + final MotionMagicVoltage controlRequest = new MotionMagicVoltage(getTargetHeight()/Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); + motor.setControl(controlRequest); SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); + SmartDashboard.putNumber("Climb/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Climb/Motor Current", motor.getStatorCurrent().getValueAsDouble()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java index 1b54558..f0e396e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -74,7 +74,18 @@ public double getCurrentHeight() { public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); } + + // atTop() and atBottom() are unused + @Override + public boolean atTop() { + return false; + } + @Override + public boolean atBottom() { + return false; + } + @Override public void periodic() { super.periodic(); @@ -84,7 +95,7 @@ public void periodic() { sim.update(Settings.DT); RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); - ElevatorVisualizer.getInstance().update(); + ElevatorVisualizer.getInstance().update(); // delete this line later SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java index 623ee97..bf7eb98 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -176,11 +176,16 @@ public void update() { // Top of Carriage is Target Height Elevator elevator = Elevator.getInstance(); - outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Constants.Elevator.MIN_HEIGHT_METERS); - outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) * Settings.Elevator.Simulation.SCALE_FACTOR + Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); - - innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); - innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + if (elevator.getTargetHeight() <= Units.inchesToMeters(47)) { + innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); + innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + } else { + outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS)); + outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) + Units.inchesToMeters(47)); + + innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); + innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); + } } From 9506dfedd0588d76704a36fc469a811025d9454f Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 11:21:13 -0500 Subject: [PATCH 3/7] fixed logging names --- .../com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 787838e..f87ffd3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -106,8 +106,8 @@ public void periodic() { motor.setControl(controlRequest); SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); - SmartDashboard.putNumber("Climb/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Climb/Motor Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Elevator/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Elevator/Motor Current", motor.getStatorCurrent().getValueAsDouble()); } } \ No newline at end of file From ae6b873de8c67c0acd79203b03283b5305bc4c37 Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 11:27:33 -0500 Subject: [PATCH 4/7] fixed commands --- .../stuypulse/robot/commands/elevator/ElevatorToBottom.java | 4 ++++ .../stuypulse/robot/commands/elevator/ElevatorToHandoff.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java | 4 ++++ .../com/stuypulse/robot/commands/elevator/ElevatorToTop.java | 4 ++++ 6 files changed, 24 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java index bc6629b..0176469 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -6,4 +6,8 @@ public class ElevatorToBottom extends ElevatorToHeight{ public ElevatorToBottom(){ super(Elevator.MIN_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java index c8e6f39..47e289b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -6,4 +6,8 @@ public class ElevatorToHandoff extends ElevatorToHeight{ public ElevatorToHandoff() { super(Elevator.HANDOFF_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java index 1d861e6..20028fa 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java @@ -6,4 +6,8 @@ public class ElevatorToLvl2 extends ElevatorToHeight{ public ElevatorToLvl2(){ super(Elevator.L2_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java index ff4a056..d0ea7ad 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java @@ -6,4 +6,8 @@ public class ElevatorToLvl3 extends ElevatorToHeight{ public ElevatorToLvl3(){ super(Elevator.L3_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java index d674636..db9b49b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java @@ -5,4 +5,8 @@ public class ElevatorToLvl4 extends ElevatorToHeight{ public ElevatorToLvl4() { super(Elevator.L4_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java index bba0cb4..2260061 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -6,5 +6,9 @@ public class ElevatorToTop extends ElevatorToHeight{ public ElevatorToTop() { super(Constants.Elevator.MAX_HEIGHT_METERS); } + + public void initialize() { + super.initialize(); + } } From 4f897188ff7a272244807125fcfe1d5b96e1d77d Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 15:40:16 -0500 Subject: [PATCH 5/7] Everything from code review: Use WPI Distance for targetHeight, config gravity type for elevator, Set target jerk to 0, reset encoder at bottom, added ElevatorToBottom() in robot init as a comment, added 3 more commands for funnel side, reworked ElevatorToHeight() Co-authored-by: BrianZ60 Co-authored-by: Baiulus Co-authored-by: Haiiden --- src/main/java/com/stuypulse/robot/Robot.java | 7 ++++- .../commands/elevator/ElevatorToBottom.java | 4 --- .../commands/elevator/ElevatorToHandoff.java | 3 -- .../commands/elevator/ElevatorToHeight.java | 22 ++++++------- .../commands/elevator/ElevatorToLvl2.java | 6 +--- .../commands/elevator/ElevatorToLvl2Alt.java | 9 ++++++ .../elevator/ElevatorToLvl2Funnel.java | 9 ++++++ .../commands/elevator/ElevatorToLvl3.java | 13 -------- .../commands/elevator/ElevatorToLvl3Alt.java | 9 ++++++ .../elevator/ElevatorToLvl3Funnel.java | 10 ++++++ .../commands/elevator/ElevatorToLvl4.java | 12 ------- .../commands/elevator/ElevatorToLvl4Alt.java | 9 ++++++ .../elevator/ElevatorToLvl4Funnel.java | 9 ++++++ .../commands/elevator/ElevatorToTop.java | 5 --- .../stuypulse/robot/constants/Constants.java | 2 ++ .../stuypulse/robot/constants/Settings.java | 20 ++++++------ .../robot/subsystems/elevator/Elevator.java | 2 +- .../subsystems/elevator/ElevatorImpl.java | 31 +++++++++++-------- .../subsystems/elevator/ElevatorSimu.java | 6 ---- 19 files changed, 104 insertions(+), 84 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java create mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e0f788d..76307d0 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,6 +5,8 @@ package com.stuypulse.robot; +import com.stuypulse.robot.commands.elevator.ElevatorToBottom; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -15,12 +17,15 @@ public class Robot extends TimedRobot { private Command auto; /*************************/ - /*** ROBOT SCHEDULEING ***/ + /*** ROBOT SCHEDULING ***/ /*************************/ @Override public void robotInit() { robot = new RobotContainer(); + + // Check with Philip to automatically reset elevator encoder + // new ElevatorToBottom().schedule(); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java index 0176469..bc6629b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java @@ -6,8 +6,4 @@ public class ElevatorToBottom extends ElevatorToHeight{ public ElevatorToBottom(){ super(Elevator.MIN_HEIGHT_METERS); } - - public void initialize() { - super.initialize(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java index 47e289b..a15139d 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHandoff.java @@ -7,7 +7,4 @@ public ElevatorToHandoff() { super(Elevator.HANDOFF_HEIGHT_METERS); } - public void initialize() { - super.initialize(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java index 28baf31..95d40c8 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java @@ -1,31 +1,29 @@ package com.stuypulse.robot.commands.elevator; -import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.subsystems.elevator.Elevator; -import com.stuypulse.stuylib.streams.booleans.BStream; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command; public class ElevatorToHeight extends InstantCommand { + + public static Command untilDone(double height) { + return new ElevatorToHeight(height) + .andThen(new WaitUntilCommand(() -> Elevator.getInstance().atTargetHeight())); + } + private final Elevator elevator; private final double targetHeight; - private final BStream belowTop, aboveBottom; public ElevatorToHeight(double targetHeight){ elevator = Elevator.getInstance(); this.targetHeight = targetHeight; - - belowTop = BStream.create(() -> !elevator.atTop()) - .and(() -> targetHeight < Constants.Elevator.MAX_HEIGHT_METERS); - aboveBottom = BStream.create(() -> !elevator.atBottom()) - .and(() -> targetHeight > Constants.Elevator.MIN_HEIGHT_METERS); addRequirements(elevator); } public void initialize(){ - if (belowTop.get() && aboveBottom.get()) { - elevator.setTargetHeight(targetHeight); - } + elevator.setTargetHeight(targetHeight); } } diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java index 20028fa..b9f1f6f 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java @@ -3,11 +3,7 @@ import com.stuypulse.robot.constants.Settings.Elevator; public class ElevatorToLvl2 extends ElevatorToHeight{ - public ElevatorToLvl2(){ + public ElevatorToLvl2Alt(){ super(Elevator.L2_HEIGHT_METERS); } - - public void initialize() { - super.initialize(); - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java new file mode 100644 index 0000000..7510446 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2Alt extends ElevatorToHeight{ + public ElevatorToLvl2Alt(){ + super(Elevator.ALT_L2_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java new file mode 100644 index 0000000..6abad77 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2Funnel.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl2Funnel extends ElevatorToHeight{ + public ElevatorToLvl2Funnel(){ + super(Elevator.FUNNEL_L2_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java deleted file mode 100644 index d0ea7ad..0000000 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3.java +++ /dev/null @@ -1,13 +0,0 @@ -package com.stuypulse.robot.commands.elevator; - -import com.stuypulse.robot.constants.Settings.Elevator; - -public class ElevatorToLvl3 extends ElevatorToHeight{ - public ElevatorToLvl3(){ - super(Elevator.L3_HEIGHT_METERS); - } - - public void initialize() { - super.initialize(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java new file mode 100644 index 0000000..59801d2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3Alt extends ElevatorToHeight{ + public ElevatorToLvl3Alt () { + super(Elevator.ALT_L3_HEIGHT_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java new file mode 100644 index 0000000..7b72017 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl3Funnel.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl3Funnel extends ElevatorToHeight{ + public ElevatorToLvl3Funnel() { + super(Elevator.FUNNEL_L3_HEIGHT_METERS); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java deleted file mode 100644 index db9b49b..0000000 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4.java +++ /dev/null @@ -1,12 +0,0 @@ -package com.stuypulse.robot.commands.elevator; -import com.stuypulse.robot.constants.Settings.Elevator; - -public class ElevatorToLvl4 extends ElevatorToHeight{ - public ElevatorToLvl4() { - super(Elevator.L4_HEIGHT_METERS); - } - - public void initialize() { - super.initialize(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java new file mode 100644 index 0000000..a8a7f38 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Alt.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4Alt extends ElevatorToHeight{ + public ElevatorToLvl4Alt(){ + super(Elevator.ALT_L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java new file mode 100644 index 0000000..8e206f0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl4Funnel.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.constants.Settings.Elevator; + +public class ElevatorToLvl4Funnel extends ElevatorToHeight{ + public ElevatorToLvl4Funnel() { + super(Elevator.FUNNEL_L4_HEIGHT_METERS); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java index 2260061..c893688 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java @@ -6,9 +6,4 @@ public class ElevatorToTop extends ElevatorToHeight{ public ElevatorToTop() { super(Constants.Elevator.MAX_HEIGHT_METERS); } - - public void initialize() { - super.initialize(); - } - } diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java index e9b30a1..bcee24f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Constants.java +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -17,6 +17,8 @@ public interface Encoders { double POSITION_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP; double VELOCITY_CONVERSION_FACTOR = MAX_HEIGHT_METERS / NUM_ROTATIONS_TO_REACH_TOP / 60; + + double DISTANCE_PER_ROTATION = 0.0; } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d36719b..82db2c1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,7 +5,6 @@ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; /*- @@ -16,7 +15,7 @@ */ public interface Settings { - double DT = 0.020; + double DT = 0.020; // 20ms Differential Time public interface Elevator { SmartNumber MAX_VELOCITY_METERS_PER_SECOND = new SmartNumber("Elevator/Max Velocity (m per s)", 1.0); @@ -24,11 +23,13 @@ public interface Elevator { // CHANGE double HANDOFF_HEIGHT_METERS = 0.1; - double L2_HEIGHT_METERS = 0.25; - double L3_HEIGHT_METERS = 0.5; - double L4_HEIGHT_METERS = 0.75; - - double FEED_HEIGHT_METERS = 0.4; + // front and funnel + double ALT_L2_HEIGHT_METERS = 0.25; + double ALT_L3_HEIGHT_METERS = 0.5; + double ALT_L4_HEIGHT_METERS = 0.75; + double FUNNEL_L2_HEIGHT_METERS = 0.3; // funnel side; should be higher than L2 + double FUNNEL_L3_HEIGHT_METERS = 0.55; // funnel side; should be higher than L3 + double FUNNEL_L4_HEIGHT_METERS = 0.8; double RAMP_RATE = 0.1; @@ -44,9 +45,10 @@ public interface Elevator { SmartNumber HEIGHT_TOLERANCE_METERS = new SmartNumber("Elevator/Height Tolerance (m)", 0.02); public interface PID { - SmartNumber kP = new SmartNumber("Elevator/Controller/kP",10); + //tune + SmartNumber kP = new SmartNumber("Elevator/Controller/kP",0.0); SmartNumber kI = new SmartNumber("Elevator/Controller/kI",0.0); - SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.2); + SmartNumber kD = new SmartNumber("Elevator/Controller/kD",0.0); } public interface FF { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index 52864cb..a92c2df 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.Robot; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Elevator extends SubsystemBase { @@ -30,7 +31,6 @@ public Elevator() { public abstract double getTargetHeight(); public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); - public abstract boolean atTop(); public abstract boolean atBottom(); public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index f87ffd3..783cd40 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.constants.Constants; @@ -11,30 +12,34 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; - +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ElevatorImpl extends Elevator { private final TalonFX motor; private final SmartNumber targetHeight; - private final DigitalInput bumpSwitchTop, bumpSwitchBottom; + private final DigitalInput bumpSwitchBottom; + private final Distance targetHeightMeters; public ElevatorImpl() { motor = new TalonFX(Ports.Elevator.MOTOR); targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); - + targetHeightMeters = Units.Meters.of(targetHeight.getAsDouble()); bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); - bumpSwitchTop = new DigitalInput(Ports.Elevator.TOP_SWITCH); TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); Slot0Configs slot0 = new Slot0Configs(); + slot0.GravityType = GravityTypeValue.Elevator_Static; + slot0.kS = Settings.Elevator.FF.kS.getAsDouble(); slot0.kV = Settings.Elevator.FF.kV.getAsDouble(); slot0.kA = Settings.Elevator.FF.kA.getAsDouble(); slot0.kG = Settings.Elevator.FF.kG.getAsDouble(); + slot0.kP = Settings.Elevator.PID.kP.getAsDouble(); slot0.kI = Settings.Elevator.PID.kI.getAsDouble(); slot0.kD = Settings.Elevator.PID.kD.getAsDouble(); @@ -57,7 +62,8 @@ public ElevatorImpl() { var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; elevatorMotionMagicConfigs.MotionMagicAcceleration = Settings.Elevator.TARGET_ACCELERATION; - elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; + elevatorMotionMagicConfigs.MotionMagicJerk = Settings.Elevator.TARGET_JERK; // Set to 0, not being used + motor.getConfigurator().apply(elevatorMotorConfig); } @@ -66,7 +72,7 @@ public ElevatorImpl() { public void setTargetHeight(double height) { targetHeight.set( SLMath.clamp( - height, + height, Constants.Elevator.MIN_HEIGHT_METERS, Constants.Elevator.MAX_HEIGHT_METERS ) @@ -75,12 +81,12 @@ public void setTargetHeight(double height) { @Override public double getTargetHeight() { - return targetHeight.getAsDouble(); + return targetHeightMeters.magnitude(); } @Override public double getCurrentHeight() { - return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR; + return motor.getPosition().getValueAsDouble() * Constants.Elevator.Encoders.DISTANCE_PER_ROTATION; } @Override @@ -93,11 +99,6 @@ public boolean atBottom() { return !bumpSwitchBottom.get(); } - @Override - public boolean atTop() { - return !bumpSwitchTop.get(); - } - @Override public void periodic() { super.periodic(); @@ -105,6 +106,10 @@ public void periodic() { final MotionMagicVoltage controlRequest = new MotionMagicVoltage(getTargetHeight()/Constants.Elevator.Encoders.POSITION_CONVERSION_FACTOR); motor.setControl(controlRequest); + if (atBottom()) { + motor.setPosition(0); + } + SmartDashboard.putNumber("Elevator/Current Height", getCurrentHeight()); SmartDashboard.putNumber("Elevator/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Elevator/Motor Current", motor.getStatorCurrent().getValueAsDouble()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java index f0e396e..58eb6a4 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -74,12 +74,6 @@ public double getCurrentHeight() { public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS.get(); } - - // atTop() and atBottom() are unused - @Override - public boolean atTop() { - return false; - } @Override public boolean atBottom() { From 3c6bf2b37102c88d75928ff4162498d7c550095d Mon Sep 17 00:00:00 2001 From: Zixi Qiao Date: Sat, 1 Feb 2025 15:40:53 -0500 Subject: [PATCH 6/7] deleted extra command --- .../robot/commands/elevator/ElevatorToLvl2.java | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java deleted file mode 100644 index b9f1f6f..0000000 --- a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToLvl2.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.elevator; - -import com.stuypulse.robot.constants.Settings.Elevator; - -public class ElevatorToLvl2 extends ElevatorToHeight{ - public ElevatorToLvl2Alt(){ - super(Elevator.L2_HEIGHT_METERS); - } -} \ No newline at end of file From bcceb78c594bf3a93ed0fe55931b16d4b8f90ff1 Mon Sep 17 00:00:00 2001 From: Baiulus Date: Mon, 3 Feb 2025 16:41:38 -0500 Subject: [PATCH 7/7] Fixed visualizer scaling, added ramp current, deleted targetHeightMeters Co-authored-by: BrianZ60 Co-authored-by: Zixi Qiao Co-authored-by: Ardian Agoes Co-authored-by: Haiiden Co-authored-by: Alvin Cheng --- .../stuypulse/robot/constants/Settings.java | 5 +- .../robot/subsystems/elevator/Elevator.java | 1 - .../subsystems/elevator/ElevatorImpl.java | 13 ++- .../elevator/ElevatorVisualizer.java | 102 ++++-------------- 4 files changed, 32 insertions(+), 89 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 82db2c1..1cc2a4a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -30,8 +30,9 @@ public interface Elevator { double FUNNEL_L2_HEIGHT_METERS = 0.3; // funnel side; should be higher than L2 double FUNNEL_L3_HEIGHT_METERS = 0.55; // funnel side; should be higher than L3 double FUNNEL_L4_HEIGHT_METERS = 0.8; - - double RAMP_RATE = 0.1; + + double RAMP_RATE_CURRENT = 1.0; + double RAMP_RATE_VOLTAGE = 0.1; // FIND OUT REAL GEAR RATIO double GEAR_RATIO = 1.0/5.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index a92c2df..d702aff 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -2,7 +2,6 @@ import com.stuypulse.robot.Robot; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Elevator extends SubsystemBase { diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 783cd40..703c5ba 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -12,8 +12,6 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,12 +19,10 @@ public class ElevatorImpl extends Elevator { private final TalonFX motor; private final SmartNumber targetHeight; private final DigitalInput bumpSwitchBottom; - private final Distance targetHeightMeters; public ElevatorImpl() { motor = new TalonFX(Ports.Elevator.MOTOR); targetHeight = new SmartNumber("Elevator/Target Height", Constants.Elevator.MIN_HEIGHT_METERS); - targetHeightMeters = Units.Meters.of(targetHeight.getAsDouble()); bumpSwitchBottom = new DigitalInput(Ports.Elevator.BOTTOM_SWITCH); TalonFXConfiguration elevatorMotorConfig = new TalonFXConfiguration(); @@ -56,8 +52,11 @@ public ElevatorImpl() { elevatorMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Ramp motor voltage - elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE; - elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE; + elevatorMotorConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE_VOLTAGE; + elevatorMotorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE_VOLTAGE; + + elevatorMotorConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod = Settings.Elevator.RAMP_RATE_CURRENT; + elevatorMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = Settings.Elevator.RAMP_RATE_CURRENT; var elevatorMotionMagicConfigs = elevatorMotorConfig.MotionMagic; elevatorMotionMagicConfigs.MotionMagicCruiseVelocity = Settings.Elevator.TARGET_CRUISE_VELOCITY; @@ -81,7 +80,7 @@ public void setTargetHeight(double height) { @Override public double getTargetHeight() { - return targetHeightMeters.magnitude(); + return targetHeight.getAsDouble(); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java index bf7eb98..692f750 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorVisualizer.java @@ -25,11 +25,8 @@ public class ElevatorVisualizer { private final MechanismRoot2d elevatorBL; private final MechanismRoot2d elevatorTR; - private final MechanismRoot2d outerBL; - private final MechanismRoot2d outerTR; - - private final MechanismRoot2d innerBL; - private final MechanismRoot2d innerTR; + private final MechanismRoot2d stageTwoBL; + private final MechanismRoot2d stageTwoTR; public static ElevatorVisualizer getInstance(){ return instance; @@ -38,7 +35,7 @@ public static ElevatorVisualizer getInstance(){ public ElevatorVisualizer() { // Mechanism2d - elevator2d = new Mechanism2d(Units.inchesToMeters(17), Units.inchesToMeters(150)); + elevator2d = new Mechanism2d(Units.inchesToMeters(14), Units.inchesToMeters(150)); // Stage One // Bottom Left Node @@ -46,7 +43,7 @@ public ElevatorVisualizer() { elevatorBL.append(new MechanismLigament2d( "Left Tower", - Units.inchesToMeters(47), + Units.inchesToMeters(39), 90, 10, new Color8Bit(Color.kOrange) @@ -55,7 +52,7 @@ public ElevatorVisualizer() { elevatorBL.append(new MechanismLigament2d( "Bottom Tower", - Units.inchesToMeters(11), + Units.inchesToMeters(10), //Change 0, 10, new Color8Bit(Color.kOrange) @@ -63,11 +60,11 @@ public ElevatorVisualizer() { ); // Top Right Node - elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(13), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + elevatorTR = elevator2d.getRoot("Elevator TR", Units.inchesToMeters(12), Units.inchesToMeters(39) + Constants.Elevator.MIN_HEIGHT_METERS); elevatorTR.append(new MechanismLigament2d( "Right Tower", - Units.inchesToMeters(47), + Units.inchesToMeters(39), -90, 10, new Color8Bit(Color.kOrange) @@ -76,7 +73,7 @@ public ElevatorVisualizer() { elevatorTR.append(new MechanismLigament2d( "Top Side", - Units.inchesToMeters(11), + Units.inchesToMeters(10), //Change 180, 10, new Color8Bit(Color.kOrange) @@ -85,20 +82,20 @@ public ElevatorVisualizer() { // Stage Two // Bottom Left Node - outerBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); + stageTwoBL = elevator2d.getRoot("Outer BL", Units.inchesToMeters(3), Constants.Elevator.MIN_HEIGHT_METERS); - outerBL.append(new MechanismLigament2d( + stageTwoBL.append(new MechanismLigament2d( "Left Side", - Units.inchesToMeters(47), + Units.inchesToMeters(35), 90, 10, new Color8Bit(Color.kYellow) ) ); - outerBL.append(new MechanismLigament2d( + stageTwoBL.append(new MechanismLigament2d( "Bottom Side", - Units.inchesToMeters(9), + Units.inchesToMeters(6), 0, 10, new Color8Bit(Color.kYellow) @@ -106,87 +103,34 @@ public ElevatorVisualizer() { ); // Top Right Node - outerTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); + stageTwoTR = elevator2d.getRoot("Outer TR", Units.inchesToMeters(12), Units.inchesToMeters(47) + Constants.Elevator.MIN_HEIGHT_METERS); - outerTR.append(new MechanismLigament2d( - "Top Side", - Units.inchesToMeters(9), - 180, - 10, - new Color8Bit(Color.kYellow) - ) - ); - - outerTR.append(new MechanismLigament2d( + stageTwoTR.append(new MechanismLigament2d( "Right Side", - Units.inchesToMeters(47), + Units.inchesToMeters(35), -90, 10, new Color8Bit(Color.kYellow) ) ); - - // Carriage - // Bottom Left Node - innerBL = elevator2d.getRoot("Inner BL", Units.inchesToMeters(4), Units.inchesToMeters(1) + Constants.Elevator.MIN_HEIGHT_METERS); - - innerBL.append(new MechanismLigament2d( - "Left Side", - Units.inchesToMeters(7), - 90, - 10, - new Color8Bit(Color.kPink) - ) - ); - - innerBL.append(new MechanismLigament2d( - "Bottom Side", - Units.inchesToMeters(7), - 0, - 10, - new Color8Bit(Color.kPink) - ) - ); - // Top Right Node - innerTR = elevator2d.getRoot("Inner TR", Units.inchesToMeters(11), Units.inchesToMeters(8) + Constants.Elevator.MIN_HEIGHT_METERS); - - innerTR.append(new MechanismLigament2d( + stageTwoTR.append(new MechanismLigament2d( "Top Side", - Units.inchesToMeters(7), + Units.inchesToMeters(6), 180, 10, - new Color8Bit(Color.kPink) - ) - ); - - innerTR.append(new MechanismLigament2d( - "Right Side", - Units.inchesToMeters(7), - -90, - 10, - new Color8Bit(Color.kPink) + new Color8Bit(Color.kYellow) ) ); - + SmartDashboard.putData("Visualizers/Elevator", elevator2d); } public void update() { - // Top of Carriage is Target Height + // Top of Stage Two is target height Elevator elevator = Elevator.getInstance(); - - if (elevator.getTargetHeight() <= Units.inchesToMeters(47)) { - innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); - innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); - } else { - outerBL.setPosition(Units.inchesToMeters(3), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS)); - outerTR.setPosition(Units.inchesToMeters(12), (elevator.getCurrentHeight() - Constants.Elevator.MIN_HEIGHT_METERS) + Units.inchesToMeters(47)); - - innerBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(1)); - innerTR.setPosition(Units.inchesToMeters(11), elevator.getCurrentHeight() + Units.inchesToMeters(8)); - } - + stageTwoBL.setPosition(Units.inchesToMeters(4), elevator.getCurrentHeight() + Units.inchesToMeters(4)); + stageTwoTR.setPosition(Units.inchesToMeters(10), elevator.getCurrentHeight() + Units.inchesToMeters(39)); } } \ No newline at end of file