Skip to content

Commit cb29ae9

Browse files
mwitcpalekmwitcpalek
committed
finish up biscuit constant tweaks for comp bot
1 parent 8ab30df commit cb29ae9

File tree

8 files changed

+64
-70
lines changed

8 files changed

+64
-70
lines changed

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

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@
5050
import frc.robot.subsystems.algae.AlgaeIOFX;
5151
import frc.robot.subsystems.algae.AlgaeSubsystem;
5252
import frc.robot.subsystems.battMon.BattMonSubsystem;
53-
import frc.robot.subsystems.biscuit.BiscuitIOFX;
53+
import frc.robot.subsystems.biscuit.BiscuitIOFXS;
5454
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
5555
import frc.robot.subsystems.climb.ClimbSubsystem;
5656
import frc.robot.subsystems.coral.CoralIO;
@@ -80,7 +80,7 @@ public class RobotContainer {
8080

8181
private final BattMonSubsystem battMonSubsystem;
8282

83-
private final BiscuitIOFX biscuitIO;
83+
private final BiscuitIOFXS biscuitIO;
8484
private final BiscuitSubsystem biscuitSubsystem;
8585

8686
private final ClimbSubsystem climbSubsystem;
@@ -117,7 +117,7 @@ public RobotContainer() {
117117

118118
battMonSubsystem = new BattMonSubsystem();
119119

120-
biscuitIO = new BiscuitIOFX();
120+
biscuitIO = new BiscuitIOFXS();
121121
biscuitSubsystem = new BiscuitSubsystem(biscuitIO);
122122

123123
climbSubsystem = new ClimbSubsystem();

‎src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,6 @@ public HoldBiscuitCommand(BiscuitSubsystem biscuitSubsystem) {
1313

1414
@Override
1515
public void initialize() {
16-
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition());
16+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition(), false);
1717
}
1818
}

‎src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,12 @@ public JogBiscuitCommand(BiscuitSubsystem biscuitSubsystem, Angle positionChange
1717

1818
@Override
1919
public void initialize() {
20-
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange));
20+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange), false);
2121
}
2222

2323
@Override
2424
public void execute() {
25-
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange));
25+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange), false);
2626
}
2727

2828
@Override

‎src/main/java/frc/robot/constants/BiscuitConstants.java‎

Lines changed: 23 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -126,14 +126,7 @@ public static TalonFXSConfiguration getFXSConfig() {
126126
.withKA(0);
127127
fxsConfig.Slot0 = slot0;
128128

129-
MotionMagicConfigs motionMagic =
130-
new MotionMagicConfigs()
131-
.withMotionMagicAcceleration(kDosntHaveAlgaeSpeed)
132-
.withMotionMagicCruiseVelocity(100)
133-
.withMotionMagicExpo_kA(0)
134-
.withMotionMagicExpo_kV(0)
135-
.withMotionMagicJerk(1000);
136-
fxsConfig.MotionMagic = motionMagic;
129+
fxsConfig.MotionMagic = getNoAlgaeMotionConfig();
137130

138131
MotorOutputConfigs motorOut =
139132
new MotorOutputConfigs()
@@ -152,4 +145,26 @@ public static TalonFXSConfiguration getFXSConfig() {
152145

153146
return fxsConfig;
154147
}
148+
149+
public static MotionMagicConfigs getAlgaeMotionConfig() {
150+
MotionMagicConfigs algaeConfig =
151+
new MotionMagicConfigs()
152+
.withMotionMagicAcceleration(500)
153+
.withMotionMagicCruiseVelocity(100)
154+
.withMotionMagicExpo_kA(0)
155+
.withMotionMagicExpo_kV(0)
156+
.withMotionMagicJerk(1000);
157+
return algaeConfig;
158+
}
159+
160+
public static MotionMagicConfigs getNoAlgaeMotionConfig() {
161+
MotionMagicConfigs algaeConfig =
162+
new MotionMagicConfigs()
163+
.withMotionMagicAcceleration(500)
164+
.withMotionMagicCruiseVelocity(100)
165+
.withMotionMagicExpo_kA(0)
166+
.withMotionMagicExpo_kV(0)
167+
.withMotionMagicJerk(1000);
168+
return algaeConfig;
169+
}
155170
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java‎

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,6 @@
33
import edu.wpi.first.units.measure.Angle;
44
import org.littletonrobotics.junction.AutoLog;
55
import org.strykeforce.telemetry.TelemetryService;
6-
import com.ctre.phoenix6.configs.MotionMagicConfigs;
76

87
public interface BiscuitIO {
98

@@ -16,11 +15,7 @@ public class BiscuitIOInputs {
1615
public boolean didZero;
1716
}
1817

19-
public default void hasAlgae(boolean enabled) {}
20-
21-
public default void doesntHaveAlgae(boolean enabled) {}
22-
23-
public default void setPosition(Angle position) {}
18+
public default void setPosition(Angle position, boolean hasAlgae) {}
2419

2520
public default void updateInputs(BiscuitIOInputs inputs) {}
2621

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java‎ renamed to ‎src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFXS.java‎

Lines changed: 12 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
import org.slf4j.LoggerFactory;
1919
import org.strykeforce.telemetry.TelemetryService;
2020

21-
public class BiscuitIOFX implements BiscuitIO {
21+
public class BiscuitIOFXS implements BiscuitIO {
2222

2323
private Logger logger;
2424
private TalonFXS talon;
@@ -33,12 +33,13 @@ public class BiscuitIOFX implements BiscuitIO {
3333
private boolean fwdLimitSwitchOpen;
3434
private Angle offset;
3535
private Alert rangeAlert = new Alert("Biscuit overextended! Shuting down!", AlertType.kError);
36+
private Boolean lastHadAlgae = false;
3637

3738
TalonFXSConfigurator configurator;
3839
private MotionMagicDutyCycle positionRequest =
3940
new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0);
4041

41-
public BiscuitIOFX() {
42+
public BiscuitIOFXS() {
4243
// Logger initialization with class name
4344
logger = LoggerFactory.getLogger(this.getClass());
4445
// Moter initialization with ID from constants
@@ -63,7 +64,15 @@ public BiscuitIOFX() {
6364
}
6465

6566
@Override
66-
public void setPosition(Angle position) {
67+
public void setPosition(Angle position, boolean hasAlgae) {
68+
if (hasAlgae != lastHadAlgae) {
69+
if (hasAlgae) {
70+
configurator.apply(BiscuitConstants.getAlgaeMotionConfig());
71+
} else {
72+
configurator.apply(BiscuitConstants.getNoAlgaeMotionConfig());
73+
}
74+
lastHadAlgae = hasAlgae;
75+
}
6776
talon.setControl(positionRequest.withPosition(position));
6877
}
6978

@@ -92,24 +101,4 @@ public void zero() {
92101
logger.info("Set Biscuit position to " + setPos);
93102
didZero = true;
94103
}
95-
96-
@Override
97-
public void hasAlgae(boolean enabled) {
98-
talonfxs
99-
.getConfigurator()
100-
.apply(
101-
BiscuitConstants.getFXSConfig()
102-
.MotionMagicConfigs
103-
.withMotionMagicAcceleration(BiscuitConstants.kHasAlgaeSpeed));
104-
}
105-
106-
@Override
107-
public void doesntHaveAlgae(boolean enabled) {
108-
talonfxs
109-
.getConfigurator()
110-
.apply(
111-
BiscuitConstants.getFXSConfig()
112-
.MotionMagicConfigs
113-
.withMotionMagicAcceleration(BiscuitConstants.kDoesntHaveAlgaeSpeed));
114-
}
115104
}

‎src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java‎

Lines changed: 3 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -9,15 +9,14 @@
99
import edu.wpi.first.units.measure.Angle;
1010
import edu.wpi.first.units.measure.AngularVelocity;
1111
import frc.robot.constants.BiscuitConstants;
12-
import frc.robot.standards.ClosedLoopPosSubsystem;
1312
import java.util.Set;
1413
import org.littletonrobotics.junction.Logger;
1514
import org.slf4j.LoggerFactory;
1615
import org.strykeforce.telemetry.TelemetryService;
1716
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
1817
import org.strykeforce.telemetry.measurable.Measure;
1918

20-
public class BiscuitSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
19+
public class BiscuitSubsystem extends MeasurableSubsystem {
2120

2221
private org.slf4j.Logger logger;
2322
private BiscuitIO io;
@@ -29,13 +28,11 @@ public BiscuitSubsystem(BiscuitIO io) {
2928
this.io = io;
3029
}
3130

32-
@Override
33-
public void setPosition(Angle position) {
34-
io.setPosition(position);
31+
public void setPosition(Angle position, boolean hasAlgae) {
32+
io.setPosition(position, hasAlgae);
3533
setPoint = position;
3634
}
3735

38-
@Override
3936
public Angle getPosition() {
4037
return Rotations.of(inputs.position);
4138
}
@@ -44,12 +41,10 @@ public AngularVelocity getVelocity(AngularVelocity velocity) {
4441
return RotationsPerSecond.of(inputs.velocity);
4542
}
4643

47-
@Override
4844
public void zero() {
4945
io.zero();
5046
}
5147

52-
@Override
5348
public boolean isFinished() {
5449
return Math.abs(getPosition().minus(setPoint).in(Rotations)) < BiscuitConstants.kCloseEnough;
5550
}

‎src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java‎

Lines changed: 19 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -243,7 +243,7 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) {
243243

244244
public void toStow() {
245245
if (biscuitSubsystem.isSafeToStow()) {
246-
biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint);
246+
biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint, hasAlgae());
247247
elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint);
248248
driveSubsystem.removeDriveMultiplier();
249249
driveSubsystem.setIgnoreSticks(false);
@@ -256,7 +256,7 @@ public void toStow() {
256256
}
257257

258258
public void toStowSafe() {
259-
biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint);
259+
biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint, hasAlgae());
260260
driveSubsystem.removeDriveMultiplier();
261261
driveSubsystem.setIgnoreSticks(false);
262262
algaeSubsystem.hold();
@@ -265,7 +265,7 @@ public void toStowSafe() {
265265
}
266266

267267
public void toStowSequential() {
268-
biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint);
268+
biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint, hasAlgae());
269269
driveSubsystem.removeDriveMultiplier();
270270
driveSubsystem.setIgnoreSticks(false);
271271
algaeSubsystem.hold();
@@ -274,7 +274,7 @@ public void toStowSequential() {
274274
}
275275

276276
private void toFunnelLoad() {
277-
biscuitSubsystem.setPosition(BiscuitConstants.kFunnelSetpoint);
277+
biscuitSubsystem.setPosition(BiscuitConstants.kFunnelSetpoint, hasAlgae());
278278
coralSubsystem.intake();
279279
elevatorSubsystem.setPosition(ElevatorConstants.kFunnelSetpoint);
280280

@@ -313,11 +313,11 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
313313

314314
switch (getAlgaeLevel()) {
315315
case L2 -> {
316-
biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeSetpoint);
316+
biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeSetpoint, hasAlgae());
317317
elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint);
318318
}
319319
case L3 -> {
320-
biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeSetpoint);
320+
biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeSetpoint, hasAlgae());
321321
elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeSetpoint);
322322
}
323323
default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
@@ -334,19 +334,19 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
334334
currentLevel = scoringLevel;
335335
switch (scoringLevel) {
336336
case L1 -> {
337-
biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint);
337+
biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint, hasAlgae());
338338
elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint);
339339
}
340340
case L2 -> {
341-
biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint);
341+
biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint, hasAlgae());
342342
elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint);
343343
}
344344
case L3 -> {
345-
biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint);
345+
biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint, hasAlgae());
346346
elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint);
347347
}
348348
case L4 -> {
349-
biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint);
349+
biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint, hasAlgae());
350350
elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint);
351351
}
352352
}
@@ -374,7 +374,7 @@ public void toAlgaeFloorPickup() {
374374
return;
375375
}
376376

377-
biscuitSubsystem.setPosition(BiscuitConstants.kFloorAlgaeSetpoint);
377+
biscuitSubsystem.setPosition(BiscuitConstants.kFloorAlgaeSetpoint, hasAlgae());
378378
elevatorSubsystem.setPosition(ElevatorConstants.kFloorAlgaeSetpoint);
379379

380380
setState(RobotStates.FLOOR_ALGAE, true);
@@ -384,7 +384,7 @@ public void toAlgaeFloorPickup() {
384384
return;
385385
}
386386

387-
biscuitSubsystem.setPosition(BiscuitConstants.kMicAlgaeSetpoint);
387+
biscuitSubsystem.setPosition(BiscuitConstants.kMicAlgaeSetpoint, hasAlgae());
388388
elevatorSubsystem.setPosition(ElevatorConstants.kMicAlgaeSetpoint);
389389

390390
setState(RobotStates.MIC_ALGAE, true);
@@ -452,7 +452,7 @@ public void toHpAlgae() {
452452

453453
algaeSubsystem.intake();
454454

455-
biscuitSubsystem.setPosition(BiscuitConstants.kHpAlgaeSetpoint);
455+
biscuitSubsystem.setPosition(BiscuitConstants.kHpAlgaeSetpoint, hasAlgae());
456456
elevatorSubsystem.setPosition(ElevatorConstants.kHpAlgaeSetpoint);
457457

458458
setState(RobotStates.HP_ALGAE, true);
@@ -464,7 +464,7 @@ private void toProcessor() {
464464
return;
465465
}
466466

467-
biscuitSubsystem.setPosition(BiscuitConstants.kProcessorSetpoint);
467+
biscuitSubsystem.setPosition(BiscuitConstants.kProcessorSetpoint, hasAlgae());
468468
elevatorSubsystem.setPosition(ElevatorConstants.kProcessorSetpoint);
469469

470470
setState(RobotStates.PROCESSOR_ALGAE, true);
@@ -476,7 +476,7 @@ public void toInterrupted() {
476476
driveSubsystem.setIgnoreSticks(false);
477477
}
478478

479-
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition());
479+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition(), hasAlgae());
480480
elevatorSubsystem.setPosition(elevatorSubsystem.getPosition());
481481

482482
setState(RobotStates.INTERRUPTED);
@@ -580,11 +580,11 @@ public void periodic() {
580580
if (algaeSubsystem.hasAlgae()) {
581581
switch (getAlgaeLevel()) {
582582
case L2 -> {
583-
biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint);
583+
biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint, hasAlgae());
584584
elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint);
585585
}
586586
case L3 -> {
587-
biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint);
587+
biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint, hasAlgae());
588588
elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint);
589589
}
590590
default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
@@ -636,7 +636,7 @@ public void periodic() {
636636
case LOADING_CORAL -> {
637637
if (coralSubsystem.hasCoral()) {
638638
coralLoc = CoralLoc.CORAL;
639-
biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint);
639+
biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint, hasAlgae());
640640
elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint);
641641
funnelSubsystem.stopMotor();
642642

@@ -674,7 +674,7 @@ public void periodic() {
674674
? BiscuitConstants.kBargeSetpoint
675675
: BiscuitConstants.kBargeBackwardSetpoint;
676676
}
677-
biscuitSubsystem.setPosition(biscuitSetpoint);
677+
biscuitSubsystem.setPosition(biscuitSetpoint, hasAlgae());
678678
curState = RobotStates.BARGE_ALGAE;
679679
}
680680
}

0 commit comments

Comments
 (0)