Skip to content

Commit 0123941

Browse files
bobopop787bobopop787
committed
teleop optimizations
- auto home to teleop - elevator higher near reef for limelight vision and dealgae - dealgae coasts - slower effector for less bounce - higher l3 - separate control constants - some shuffleboard changes
1 parent d327806 commit 0123941

File tree

6 files changed

+180
-103
lines changed

6 files changed

+180
-103
lines changed

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 21 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,7 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
115115
0
116116
);
117117

118-
public static final ControlConstants kPresetPosControlConstants = new ControlConstants(
118+
public static final ControlConstants kPresetPosXControlConstants = new ControlConstants(
119119
"SwervePresetPos",
120120
10, // TODO probably turn up
121121
0,
@@ -125,6 +125,16 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
125125
0
126126
);
127127

128+
public static final ControlConstants kPresetPosYControlConstants = new ControlConstants(
129+
"SwervePresetPos",
130+
10, // TODO probably turn up
131+
0,
132+
0,
133+
0,
134+
0,
135+
0
136+
);
137+
128138
static {
129139
// Preferences.initDouble("kSwerveTestTurn", 0);
130140
// Preferences.initDouble("kSwerveTestDrive", 0);
@@ -231,6 +241,7 @@ public static class ElevatorConstants { // CAN ID range 11-19
231241
public static final SparkMaxConfig elevatorLeaderConfig = new SparkMaxConfig();
232242
public static final SparkMaxConfig elevatorFollowerConfig = new SparkMaxConfig();
233243
public static final SparkMaxConfig effectorConfig = new SparkMaxConfig(); // also used for funnel motor, since is the same
244+
public static final SparkMaxConfig dealgaeConfig = new SparkMaxConfig();
234245

235246
static {
236247
elevatorLeaderConfig
@@ -277,6 +288,15 @@ public static class ElevatorConstants { // CAN ID range 11-19
277288
.positionConversionFactor(1) // revolutions
278289
.velocityConversionFactor(1) // RPM
279290
;
291+
292+
dealgaeConfig
293+
.idleMode(IdleMode.kCoast)
294+
.smartCurrentLimit(30)
295+
.voltageCompensation(12)
296+
; dealgaeConfig.encoder
297+
.positionConversionFactor(1) // revolutions
298+
.velocityConversionFactor(1) // RPM
299+
;
280300
}
281301
}
282302

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -100,8 +100,8 @@ private void configureBindings() {
100100
);
101101
overrideElevatorFactorCombo.getTrigger().onTrue(Commands.runOnce(() -> {}));
102102
Shuffleboard.getTab("Teleoperated")
103-
.addBoolean("EleFact Override", overrideElevatorFactorCombo.getTrigger()).
104-
withPosition(8, 0)
103+
.addBoolean("EleFact Override", overrideElevatorFactorCombo.getTrigger())
104+
.withPosition(8, 2)
105105
.withSize(1, 1);
106106

107107
swerve.setToAimSuppliers(
@@ -155,7 +155,7 @@ private void configureBindings() {
155155

156156
// driverController.povUp().onTrue(elevator.runEffectorPreferences());
157157
// driverController.povUp().onFalse(elevator.runEffector(0, 0));
158-
// driverController.povDown().onTrue(elevator.runaEffectorPreferences());
158+
// driverController.povDown().onTrue(elevator.runReversedEffectorPreferences());
159159
// driverController.povDown().onFalse(elevator.runEffector(0, 0));
160160

161161
// driverController.povLeft().onTrue(elevator.runElevatorOpenLoopPreferences());
@@ -173,18 +173,18 @@ private void configureBindings() {
173173
.addBoolean("Aligned Override", alignedOverrideCombo.getTrigger()).
174174
withPosition(3, 2)
175175
.withSize(1, 1);
176-
elevator.setIsAlignedSupplier(swerve::isAligned);
176+
elevator.setIsAlignedSupplier(() -> swerve.isAligned() || alignedOverrideCombo.getTrigger().getAsBoolean());
177+
elevator.setScoreComboSupplier(auxController.rightTrigger());
178+
elevator.setErrorDistanceSupplier(swerve::getErrorDistance);
177179

178180
auxController.b().onTrue(elevator.runToElevatorPosition(ElevatorPosition.HOME));
179181
auxController.a().onTrue(elevator.runToElevatorPosition(ElevatorPosition.L1));
180182
auxController.x().onTrue(elevator.runToElevatorPosition(ElevatorPosition.L2));
181183
auxController.y().onTrue(elevator.runToElevatorPosition(ElevatorPosition.L3));
182184
auxController.leftBumper().onTrue(elevator.runIntakeEffector(
183185
// TODO slow to 4
184-
5, // effector volts
185-
-2, // funnel volts
186-
() -> swerve.isAligned() || alignedOverrideCombo.getTrigger().getAsBoolean() // is aligned supplier
187-
// () -> true
186+
4, // effector volts
187+
-2 // funnel volts
188188
));
189189
auxController.leftBumper().onFalse(elevator.runStopIntakeEffector());
190190

‎src/main/java/frc/robot/subsystems/elevator/Elevator.java‎

Lines changed: 119 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package frc.robot.subsystems.elevator;
22

33
import java.util.function.BooleanSupplier;
4+
import java.util.function.DoubleSupplier;
45

56
import org.littletonrobotics.junction.Logger;
67

@@ -19,26 +20,36 @@ public class Elevator extends SubsystemBase {
1920
private boolean openLoop;
2021

2122
private boolean intaking;
23+
private boolean ejecting;
2224
private boolean bouncing;
2325

2426
private double desiredLeftVolts;
2527
private double desiredRightVolts;
2628
private double desiredFunnelVolts;
2729
private double lastUpperPhotosensorTrigger;
30+
private double lastLowerPhotosensorTrigger;
2831

2932
private BooleanSupplier isAlignedSupplier;
33+
private BooleanSupplier scoreComboSupplier;
34+
private boolean awaitingScoreCombo;
3035
private boolean dealgaeRunning;
3136

37+
private DoubleSupplier errorDistanceSupplier;
38+
3239
public Elevator(ElevatorIO io) {
3340
this.elevatorIO = io;
3441
elevatorIOInputs = new ElevatorIOInputsAutoLogged();
3542
openLoop = false;
3643
intaking = false;
44+
ejecting = false;
3745
bouncing = true;
3846
dealgaeRunning = false;
47+
awaitingScoreCombo = false;
3948
desiredLeftVolts = 0;
4049
desiredRightVolts = 0;
4150
desiredFunnelVolts = 0;
51+
lastUpperPhotosensorTrigger = 0;
52+
lastLowerPhotosensorTrigger = 0;
4253
Preferences.initDouble("ele/leftvolts", 0);
4354
Preferences.initDouble("ele/rightvolts", 0);
4455
Preferences.initDouble("ele/elevatorvolts", 0);
@@ -63,14 +74,25 @@ public Elevator(ElevatorIO io) {
6374
.withPosition(2, 2)
6475
.withSize(1, 1);
6576
teleopTab.addBoolean("Home Bounce", () -> bouncing)
66-
.withPosition(7, 0)
77+
.withPosition(7, 2)
78+
.withSize(1, 1);
79+
teleopTab.addBoolean("Awaiting Home", () -> awaitingScoreCombo)
80+
.withPosition(7, 1)
6781
.withSize(1, 1);
6882
}
6983

7084
public void setIsAlignedSupplier(BooleanSupplier isAlignedSupplier) {
7185
this.isAlignedSupplier = isAlignedSupplier;
7286
}
7387

88+
public void setScoreComboSupplier(BooleanSupplier scoreComboSupplier) {
89+
this.scoreComboSupplier = scoreComboSupplier;
90+
}
91+
92+
public void setErrorDistanceSupplier(DoubleSupplier errorDistanceSupplier) {
93+
this.errorDistanceSupplier = errorDistanceSupplier;
94+
}
95+
7496
public Command runElevatorOpenLoop(double volts) {
7597
return runOnce(() -> {
7698
openLoop = true;
@@ -132,7 +154,7 @@ public Command runEffectorPreferences() {
132154
});
133155
}
134156

135-
public Command runaEffectorPreferences() {
157+
public Command runReversedEffectorPreferences() {
136158
return runOnce(() -> {
137159
desiredLeftVolts = -Preferences.getDouble("ele/leftvolts", 0);
138160
desiredRightVolts = -Preferences.getDouble("ele/rightvolts", 0);
@@ -175,66 +197,6 @@ public Command runToggleBouncing() {
175197
});
176198
}
177199

178-
@Override
179-
public void periodic() {
180-
elevatorIO.updateInputs(elevatorIOInputs);
181-
Logger.processInputs("Elevator", elevatorIOInputs);
182-
183-
if(elevatorIOInputs.lowLimit) {
184-
elevatorIO.resetElevatorEncoder(0);
185-
} else if(elevatorIOInputs.highLimit) {
186-
elevatorIO.resetElevatorEncoder(0.612);
187-
}
188-
if(!openLoop) {
189-
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) && !hasCoral() && bouncing) {
190-
elevatorIO.setElevatorPosition(ElevatorPosition.HOME.height + (0.005 * Math.sin(Timer.getFPGATimestamp() * 12)));
191-
} else elevatorIO.setElevatorPosition(elevatorIOInputs.desiredHeight);
192-
// elevatorIO.setElevatorPosition(elevatorIOInputs.desiredPosition);
193-
}
194-
195-
double realDesiredHeight = Math.max(Math.min(elevatorIOInputs.desiredHeight, 0.612), 0);
196-
if(desiredLeftVolts == 0) {
197-
elevatorIO.setLeftEffectorVolts(desiredLeftVolts);
198-
} else if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) ||
199-
(Math.abs(realDesiredHeight - elevatorIOInputs.elevatorHeight) < 0.0015 &&
200-
(isAlignedSupplier == null || isAlignedSupplier.getAsBoolean())
201-
)
202-
) {
203-
elevatorIO.setLeftEffectorVolts(desiredLeftVolts);
204-
} else {
205-
elevatorIO.setLeftEffectorVolts(0);
206-
}
207-
if(desiredRightVolts == 0) {
208-
elevatorIO.setRightEffectorVolts(desiredRightVolts);
209-
} else if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) ||
210-
(Math.abs(realDesiredHeight - elevatorIOInputs.elevatorHeight) < 0.0015 &&
211-
(isAlignedSupplier == null || isAlignedSupplier.getAsBoolean())
212-
)
213-
) {
214-
elevatorIO.setRightEffectorVolts(desiredRightVolts);
215-
} else {
216-
elevatorIO.setRightEffectorVolts(0);
217-
}
218-
219-
if(elevatorIOInputs.upperPhotosensor) {
220-
lastUpperPhotosensorTrigger = Timer.getFPGATimestamp();
221-
}
222-
if(Timer.getFPGATimestamp() - lastUpperPhotosensorTrigger < 1.5 && !elevatorIOInputs.lowerPhotosensor) {
223-
elevatorIO.setFunnelMotorVolts(Timer.getFPGATimestamp() % 6 > 1.5 && Timer.getFPGATimestamp() % 1.5 > 0.75 ? -desiredFunnelVolts : desiredFunnelVolts);
224-
} else {
225-
elevatorIO.setFunnelMotorVolts(desiredFunnelVolts);
226-
}
227-
228-
if(!dealgaeRunning && elevatorIOInputs.elevatorHeight > 0.05) {
229-
dealgaeRunning = true;
230-
elevatorIO.setDealgaeMotorVolts(6);
231-
}
232-
if(dealgaeRunning && elevatorIOInputs.elevatorHeight < 0.05) {
233-
dealgaeRunning = false;
234-
elevatorIO.setDealgaeMotorVolts(0);
235-
}
236-
}
237-
238200
/**
239201
* Moves elevator up, waits until elevator is (almost) at position, expels coral, waits until it's placed, stops end effector, then lowers elevator.
240202
* @param position Elevator height to raise elevator to and score at.
@@ -273,7 +235,7 @@ public Command runIntakeFromCoralStation() {
273235
.andThen(runSetFunnelVolts(0));
274236
}
275237

276-
public Command runIntakeEffector(double effectorVolts, double funnelVolts, BooleanSupplier isAlignedSupplier) {
238+
public Command runIntakeEffector(double effectorVolts, double funnelVolts) {
277239
return runOnce(() -> {
278240
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) && intaking) {
279241
intaking = false;
@@ -290,12 +252,11 @@ public Command runIntakeEffector(double effectorVolts, double funnelVolts, Boole
290252
desiredLeftVolts = -effectorVolts;
291253
desiredRightVolts = effectorVolts;
292254
// elevatorIO.setEffectorVolts(-effectorVolts, effectorVolts);
293-
} else if(isAlignedSupplier.getAsBoolean()) {
294-
// TODO make it hold down still work
295-
// } else if(isAlignedSupplier.getAsBoolean() && Math.abs(elevatorIOInputs.elevatorHeight - elevatorIOInputs.desiredHeight) < 0.0015) {
255+
} else {
256+
awaitingScoreCombo = scoreComboSupplier.getAsBoolean();
296257
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.L1)) {
297-
desiredLeftVolts = -effectorVolts * 6 / 7;
298-
desiredRightVolts = effectorVolts * 3 / 7;
258+
desiredLeftVolts = -effectorVolts * 6 / 7 * 5 / 4;
259+
desiredRightVolts = effectorVolts * 3 / 7 * 5 / 4;
299260
// elevatorIO.setEffectorVolts(-effectorVolts * 6 / 7, effectorVolts * 3 / 7); // over 5 to over 7
300261
} else {
301262
desiredLeftVolts = -effectorVolts;
@@ -329,4 +290,94 @@ public Command runWaitStopIntake() {
329290
.andThen(runSetFunnelVolts(0))
330291
.andThen(runOnce(() -> intaking = false)));
331292
}
293+
294+
@Override
295+
public void periodic() {
296+
elevatorIO.updateInputs(elevatorIOInputs);
297+
Logger.processInputs("Elevator", elevatorIOInputs);
298+
299+
if(elevatorIOInputs.lowLimit) {
300+
elevatorIO.resetElevatorEncoder(0);
301+
} else if(elevatorIOInputs.highLimit) {
302+
elevatorIO.resetElevatorEncoder(0.612);
303+
}
304+
if(!openLoop) {
305+
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) && !hasCoral() && bouncing) {
306+
elevatorIO.setElevatorPosition(ElevatorPosition.HOME.height + (0.005 * Math.sin(Timer.getFPGATimestamp() * 12)));
307+
} else {
308+
if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) && bouncing) {
309+
elevatorIO.setElevatorPosition(errorDistanceSupplier.getAsDouble() < 0.5 ? ElevatorPosition.homeCoralPos : ElevatorPosition.HOME.height);
310+
} else {
311+
elevatorIO.setElevatorPosition(elevatorIOInputs.desiredHeight);
312+
}
313+
}
314+
// elevatorIO.setElevatorPosition(elevatorIOInputs.desiredPosition);
315+
}
316+
317+
double realDesiredHeight = Math.max(Math.min(elevatorIOInputs.desiredHeight, 0.612), 0);
318+
if(desiredLeftVolts == 0) {
319+
elevatorIO.setLeftEffectorVolts(desiredLeftVolts);
320+
} else if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) ||
321+
(Math.abs(realDesiredHeight - elevatorIOInputs.elevatorHeight) < 0.0015 &&
322+
(isAlignedSupplier == null || isAlignedSupplier.getAsBoolean())
323+
)
324+
) {
325+
elevatorIO.setLeftEffectorVolts(desiredLeftVolts);
326+
} else {
327+
elevatorIO.setLeftEffectorVolts(0);
328+
}
329+
if(desiredRightVolts == 0) {
330+
elevatorIO.setRightEffectorVolts(desiredRightVolts);
331+
} else if(elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) ||
332+
(Math.abs(realDesiredHeight - elevatorIOInputs.elevatorHeight) < 0.0015 &&
333+
(isAlignedSupplier == null || isAlignedSupplier.getAsBoolean())
334+
)
335+
) {
336+
elevatorIO.setRightEffectorVolts(desiredRightVolts);
337+
} else {
338+
elevatorIO.setRightEffectorVolts(0);
339+
}
340+
341+
if(elevatorIOInputs.upperPhotosensor) {
342+
lastUpperPhotosensorTrigger = Timer.getFPGATimestamp();
343+
}
344+
if(elevatorIOInputs.lowerPhotosensor) {
345+
lastLowerPhotosensorTrigger = Timer.getFPGATimestamp();
346+
}
347+
348+
if(Timer.getFPGATimestamp() - lastUpperPhotosensorTrigger < 0.5 && !elevatorIOInputs.lowerPhotosensor) {
349+
elevatorIO.setFunnelMotorVolts(Timer.getFPGATimestamp() % 6 > 1.5 && Timer.getFPGATimestamp() % 1.5 > 0.75 ? -desiredFunnelVolts : desiredFunnelVolts);
350+
} else {
351+
elevatorIO.setFunnelMotorVolts(desiredFunnelVolts);
352+
}
353+
354+
if(!dealgaeRunning && elevatorIOInputs.elevatorHeight > 0.01) {
355+
dealgaeRunning = true;
356+
elevatorIO.setDealgaeMotorVolts(9);
357+
}
358+
if(dealgaeRunning && elevatorIOInputs.elevatorHeight < 0.01) {
359+
dealgaeRunning = false;
360+
elevatorIO.setDealgaeMotorVolts(0);
361+
}
362+
363+
if(
364+
awaitingScoreCombo &&
365+
!elevatorIOInputs.desiredPosition.equals(ElevatorPosition.HOME) &&
366+
Timer.getFPGATimestamp() - lastLowerPhotosensorTrigger > 0.3 &&
367+
Math.abs(realDesiredHeight - elevatorIOInputs.elevatorHeight) < 0.0015
368+
) {
369+
awaitingScoreCombo = false;
370+
elevatorIO.setElevatorPosition(ElevatorPosition.HOME);
371+
}
372+
373+
Logger.recordOutput("Elevator/intaking", intaking);
374+
Logger.recordOutput("Elevator/bouncing", bouncing);
375+
Logger.recordOutput("Elevator/subdesiredLeftVolts", desiredLeftVolts);
376+
Logger.recordOutput("Elevator/subdesiredRightVolts", desiredRightVolts);
377+
Logger.recordOutput("Elevator/subdesiredFunnelVolts", desiredFunnelVolts);
378+
Logger.recordOutput("Elevator/lastUpperPhotosensorTrigger", lastUpperPhotosensorTrigger);
379+
Logger.recordOutput("Elevator/lastLowerPhotosensorTrigger", lastLowerPhotosensorTrigger);
380+
Logger.recordOutput("Elevator/awaitingScoreCombo", awaitingScoreCombo);
381+
Logger.recordOutput("Elevator/dealgaeRunning", dealgaeRunning);
382+
}
332383
}

‎src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ public ElevatorIOSpark() {
7676
leftEffectorMotor.configure(ElevatorConstants.effectorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
7777
rightEffectorMotor.configure(ElevatorConstants.effectorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
7878
funnelMotor.configure(ElevatorConstants.effectorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
79-
dealgaeMotor.configure(ElevatorConstants.effectorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
79+
dealgaeMotor.configure(ElevatorConstants.dealgaeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
8080

8181
highLimit = new DigitalInput(ElevatorConstants.kHighLimitDIO);
8282
lowLimit = new DigitalInput(ElevatorConstants.kLowLimitDIO);

‎src/main/java/frc/robot/subsystems/elevator/ElevatorPosition.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,13 +4,15 @@ public enum ElevatorPosition {
44
HOME("Home", 0.005),
55
L1("L1", 0.327),
66
L2("L2", 0.452),
7-
L3("L3", 0.609),
7+
L3("L3", 0.612),
88
// L4("L4", 0.0),
99
;
1010

1111
public final String name;
1212
public final double height;
1313

14+
public final static double homeCoralPos = 0.073;
15+
1416
private ElevatorPosition(String name, double height) {
1517
this.name = name;
1618
this.height = height;

0 commit comments

Comments
 (0)