Skip to content

Commit 9c581e5

Browse files
dabeycorndabeycorn
committed
feat: create elevator public interface
Signed-off-by: Dasun Abeykoon <[email protected]>
1 parent 02b5350 commit 9c581e5

File tree

2 files changed

+203
-6
lines changed

2 files changed

+203
-6
lines changed

‎src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmPivot.java‎

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -47,10 +47,9 @@ public class ArmPivot extends SubsystemBase implements AutoCloseable {
4747

4848
private final LinearFilter pivotCurrentFilter = LinearFilter.movingAverage(5);
4949

50-
@Logged(name = "Filted Pivot Motor Current (Amps)")
50+
@Logged(name = "Filted Pivot Motor Stator Current (Amps)")
5151
private double filteredPivotCurrent;
5252

53-
@Logged(name = "Is Zeroed (bool)")
5453
private boolean isZeroed = false;
5554

5655
private final Mechanism2d canvas =
@@ -78,6 +77,14 @@ public void periodic() {
7877
Rotation2d.fromRadians(hardware.getAngleRads()).unaryMinus().rotateBy(Rotation2d.k180deg));
7978
}
8079

80+
/**
81+
* @return true if arm has been homed
82+
*/
83+
@Logged(name = "Is Zeroed (bool)")
84+
public boolean isZeroed() {
85+
return isZeroed;
86+
}
87+
8188
/**
8289
* @return true if arm is within a certain tolerance of the setpoint angle
8390
*/
@@ -94,10 +101,7 @@ public boolean isNearSetpointAngle() {
94101
*/
95102
// TODO CHECK VALUES
96103
public Command runCurrentHoming() {
97-
return this.run(
98-
() -> {
99-
hardware.setVolts(-2.5);
100-
})
104+
return this.run(() -> hardware.setVolts(-2.5))
101105
.until(() -> Math.abs(filteredPivotCurrent) > 50.0)
102106
.finallyDo(
103107
(interupted) -> {

‎src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/Elevator.java‎

Lines changed: 193 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,193 @@
1+
// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
2+
// https://github.com/wmironpatriots
3+
//
4+
// Open Source Software; you can modify and/or share it under the terms of
5+
// MIT license file in the root directory of this project
6+
7+
package org.frc6423.robot.subsystems.superstructure.elevator;
8+
9+
import static edu.wpi.first.units.Units.Centimeters;
10+
import static edu.wpi.first.units.Units.Meters;
11+
import static edu.wpi.first.units.Units.MetersPerSecond;
12+
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
13+
14+
import edu.wpi.first.epilogue.Logged;
15+
import edu.wpi.first.math.MathUtil;
16+
import edu.wpi.first.math.filter.LinearFilter;
17+
import edu.wpi.first.math.geometry.Pose3d;
18+
import edu.wpi.first.units.measure.Distance;
19+
import edu.wpi.first.units.measure.LinearAcceleration;
20+
import edu.wpi.first.units.measure.LinearVelocity;
21+
import edu.wpi.first.wpilibj2.command.Command;
22+
import edu.wpi.first.wpilibj2.command.Commands;
23+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
24+
import java.util.function.DoubleSupplier;
25+
26+
/** Elevator Subsystem */
27+
public class Elevator extends SubsystemBase {
28+
/** CONSTANTS */
29+
// TODO
30+
public static final double GEAR_RATIO = 0.0;
31+
32+
// TODO
33+
public static final Distance MAX_EXTENSION_HEIGHT = Meters.of(0.0);
34+
public static final Distance TOLERANCE = Centimeters.of(5);
35+
36+
public static final LinearVelocity MAX_VELOCITY = MetersPerSecond.of(4.5);
37+
public static final LinearAcceleration MAX_ACCELERATION = MetersPerSecondPerSecond.of(10.0);
38+
public static final LinearAcceleration SLOW_ACCELERATION = MetersPerSecondPerSecond.of(5.0);
39+
40+
@Logged(name = "Elevator Hardware Loggables")
41+
private final ElevatorIO hardware;
42+
43+
private final LinearFilter currentFilter = LinearFilter.movingAverage(5);
44+
45+
@Logged(name = "Filtered Parent Motor Stator Current (Amps)")
46+
private double filteredCurrent;
47+
48+
private boolean isZeroed = false;
49+
50+
public Elevator(ElevatorIO hardware) {
51+
this.hardware = hardware;
52+
}
53+
54+
@Override
55+
public void periodic() {
56+
hardware.periodic();
57+
58+
filteredCurrent = currentFilter.calculate(hardware.getParentStatorCurrentAmps());
59+
}
60+
61+
/**
62+
* @return true if elevator has been zeroed
63+
*/
64+
@Logged(name = "Is Zeroed (bool)")
65+
public boolean isZeroed() {
66+
return isZeroed;
67+
}
68+
69+
/**
70+
* @return true if elevator is within a certain tolerance of the setpoint pose
71+
*/
72+
@Logged(name = "Near Setpoint Pose (bool)")
73+
public boolean isNearSetpointPose() {
74+
return MathUtil.isNear(
75+
hardware.getSetpointPoseMeters(), hardware.getParentPoseMeters(), TOLERANCE.in(Meters));
76+
}
77+
78+
// TODO
79+
@Logged(name = "Carriage (Pose3d)")
80+
public Pose3d getCarriagePose3d() {
81+
return Pose3d.kZero;
82+
}
83+
84+
// TODO
85+
@Logged(name = "First Stage (Pose3d)")
86+
public Pose3d getFirstStagePose3d() {
87+
return Pose3d.kZero;
88+
}
89+
90+
/**
91+
* Run elevator into hardstop to determine home (aka, zero)
92+
*
93+
* @return {@link Command}
94+
*/
95+
public Command runCurrentHoming() {
96+
return this.run(() -> hardware.setVolts(-2.5))
97+
.until(() -> Math.abs(filteredCurrent) > 50.0)
98+
.finallyDo(
99+
(interupted) -> {
100+
if (!interupted) {
101+
hardware.resetEncoders(0.0);
102+
isZeroed = true;
103+
}
104+
});
105+
}
106+
107+
/**
108+
* Run elevator to specified extension
109+
*
110+
* @param extension {@link Extension} representing desired extension
111+
* @return {@link Command}
112+
*/
113+
public Command runExtension(Distance extension) {
114+
return this.run(
115+
() ->
116+
hardware.setPose(extension.in(Meters), MAX_ACCELERATION.in(MetersPerSecondPerSecond)));
117+
}
118+
119+
/**
120+
* Run elevator to specified extension
121+
*
122+
* @param extension desired extension in meters
123+
* @return {@link Command}
124+
*/
125+
public Command runExtension(DoubleSupplier extensionMeters) {
126+
return this.run(
127+
() ->
128+
hardware.setPose(
129+
extensionMeters.getAsDouble(), MAX_ACCELERATION.in(MetersPerSecondPerSecond)));
130+
}
131+
132+
/**
133+
* Run elevator to specified extension
134+
*
135+
* @param extension desired extension in meters
136+
* @return {@link Command}
137+
*/
138+
public Command runExtension(double extensionMeters) {
139+
return runExtension(() -> extensionMeters);
140+
}
141+
142+
/**
143+
* Run elevator to specified extension
144+
*
145+
* @param extension {@link Extension} representing desired extension
146+
* @return {@link Command}
147+
*/
148+
public Command runSlowExtension(Distance extension) {
149+
return this.run(
150+
() ->
151+
hardware.setPose(extension.in(Meters), SLOW_ACCELERATION.in(MetersPerSecondPerSecond)));
152+
}
153+
154+
/**
155+
* Run elevator to specified extension
156+
*
157+
* @param extension desired extension in meters
158+
* @return {@link Command}
159+
*/
160+
public Command runSlowExtension(DoubleSupplier extension) {
161+
return this.run(
162+
() ->
163+
hardware.setPose(
164+
extension.getAsDouble(), SLOW_ACCELERATION.in(MetersPerSecondPerSecond)));
165+
}
166+
167+
/**
168+
* Run elevator to specified extension
169+
*
170+
* @param extension desired extension in meters
171+
* @return {@link Command}
172+
*/
173+
public Command runSlowExtension(double extension) {
174+
return runSlowExtension(() -> extension);
175+
}
176+
177+
/**
178+
* Hold elevator at current extension
179+
*
180+
* @return {@link Command}
181+
*/
182+
public Command holdExtension() {
183+
return Commands.sequence(
184+
this.run(
185+
() -> {
186+
var currentPose = hardware.getParentPoseMeters();
187+
188+
hardware.setPose(currentPose, SLOW_ACCELERATION.in(MetersPerSecondPerSecond));
189+
})
190+
.until(() -> true),
191+
this.run(() -> {}));
192+
}
193+
}

0 commit comments

Comments
 (0)