Skip to content

Commit daf150d

Browse files
JetblackdragonJetblackdragon
authored
Switch drivebase from openloop voltage to velocity control. (#192)
1 parent 4eba78a commit daf150d

File tree

10 files changed

+705
-1027
lines changed

10 files changed

+705
-1027
lines changed

‎src/main/deploy/elastic-layout.json‎

Lines changed: 320 additions & 850 deletions
Large diffs are not rendered by default.

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

Lines changed: 79 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
import frc.robot.util.BranchHeight;
3838
import frc.robot.util.RobotType;
3939
import frc.robot.util.ScoringMode;
40+
import frc.robot.util.ScoringType;
4041
import frc.robot.util.SoloScoringMode;
4142
import java.util.function.BooleanSupplier;
4243

@@ -83,8 +84,7 @@ public class Controls {
8384
new SwerveRequest.FieldCentric()
8485
.withDeadband(0.0001)
8586
.withRotationalDeadband(0.0001)
86-
.withDriveRequestType(
87-
DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
87+
.withDriveRequestType(DriveRequestType.Velocity);
8888

8989
private final Telemetry logger = new Telemetry(MaxSpeed);
9090

@@ -112,6 +112,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) {
112112
configureGroundSpinnyBindings();
113113
configureGroundArmBindings();
114114
configureSoloControllerBindings();
115+
Shuffleboard.getTab("Elevator")
116+
.addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE);
117+
Shuffleboard.getTab("Elevator")
118+
.addString("Scoring Mode", () -> getSoloScoringMode().toString());
119+
}
120+
121+
public SoloScoringMode getSoloScoringMode() {
122+
return soloScoringMode;
115123
}
116124

117125
private Trigger connected(CommandXboxController controller) {
@@ -287,7 +295,7 @@ private void configureSuperStructureBindings() {
287295
Commands.deferredProxy(
288296
() ->
289297
switch (scoringMode) {
290-
case CORAL -> getCoralBranchHeightCommand();
298+
case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER);
291299
case ALGAE -> Commands.sequence(
292300
superStructure.algaeProcessorScore(
293301
driverController.rightBumper()),
@@ -394,12 +402,7 @@ private void configureSuperStructureBindings() {
394402
case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW;
395403
}
396404
})
397-
.withName("Set solo scoring mode"));
398-
399-
sensors
400-
.armSensor
401-
.inClaw()
402-
.and(RobotModeTriggers.teleop())
405+
.withName("Set solo scoring mode"))
403406
.onFalse(
404407
Commands.runOnce(
405408
() -> {
@@ -423,7 +426,7 @@ private void configureSuperStructureBindings() {
423426
() -> {
424427
Command scoreCommand =
425428
switch (scoringMode) {
426-
case CORAL -> getCoralBranchHeightCommand();
429+
case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER);
427430
case ALGAE -> Commands.sequence(
428431
BargeAlign.bargeScore(
429432
s.drivebaseSubsystem,
@@ -448,30 +451,55 @@ private Command getAlgaeIntakeCommand() {
448451
};
449452
}
450453

451-
private Command getCoralBranchHeightCommand() {
452-
return switch (branchHeight) {
453-
case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper());
454-
case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper());
455-
case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper());
456-
case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper());
457-
};
458-
}
459-
460-
private Command getSoloCoralBranchHeightCommand() {
461-
return switch (branchHeight) {
462-
case CORAL_LEVEL_FOUR -> superStructure
463-
.coralLevelFour(soloController.rightBumper())
464-
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
465-
case CORAL_LEVEL_THREE -> superStructure
466-
.coralLevelThree(soloController.rightBumper())
467-
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
468-
case CORAL_LEVEL_TWO -> superStructure
469-
.coralLevelTwo(soloController.rightBumper())
470-
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
471-
case CORAL_LEVEL_ONE -> superStructure
472-
.coralLevelOne(soloController.rightBumper())
473-
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
474-
};
454+
private Command getCoralBranchHeightCommand(ScoringType version) {
455+
if (version == ScoringType.SOLOC_LEFT) {
456+
return switch (branchHeight) {
457+
case CORAL_LEVEL_FOUR -> superStructure
458+
.coralLevelFour(soloController.rightBumper())
459+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
460+
case CORAL_LEVEL_THREE -> superStructure
461+
.coralLevelThree(
462+
soloController.rightBumper(),
463+
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB))
464+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
465+
case CORAL_LEVEL_TWO -> superStructure
466+
.coralLevelTwo(
467+
soloController.rightBumper(),
468+
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB))
469+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
470+
case CORAL_LEVEL_ONE -> superStructure
471+
.coralLevelOne(
472+
soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB))
473+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
474+
};
475+
} else if (version == ScoringType.SOLOC_RIGHT) {
476+
return switch (branchHeight) {
477+
case CORAL_LEVEL_FOUR -> superStructure
478+
.coralLevelFour(soloController.rightBumper())
479+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
480+
case CORAL_LEVEL_THREE -> superStructure
481+
.coralLevelThree(
482+
soloController.rightBumper(),
483+
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB))
484+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
485+
case CORAL_LEVEL_TWO -> superStructure
486+
.coralLevelTwo(
487+
soloController.rightBumper(),
488+
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB))
489+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
490+
case CORAL_LEVEL_ONE -> superStructure
491+
.coralLevelOne(
492+
soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB))
493+
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
494+
};
495+
} else {
496+
return switch (branchHeight) {
497+
case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper());
498+
case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper());
499+
case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper());
500+
case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper());
501+
};
502+
}
475503
}
476504

477505
private void configureElevatorBindings() {
@@ -806,12 +834,12 @@ private void configureAutoAlignBindings() {
806834
.rightTrigger()
807835
.and(() -> scoringMode == ScoringMode.CORAL)
808836
.and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
809-
.whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this));
837+
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB));
810838
driverController
811839
.leftTrigger()
812840
.and(() -> scoringMode == ScoringMode.CORAL)
813841
.and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
814-
.whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this));
842+
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB));
815843
}
816844

817845
private void configureGroundSpinnyBindings() {
@@ -926,7 +954,7 @@ private void configureSoloControllerBindings() {
926954
switch (soloScoringMode) {
927955
case CORAL_IN_CLAW -> {
928956
scoreCommand =
929-
getSoloCoralBranchHeightCommand()
957+
getCoralBranchHeightCommand(ScoringType.SOLOC_LEFT)
930958
.until(
931959
() ->
932960
soloController.a().getAsBoolean()
@@ -968,7 +996,12 @@ private void configureSoloControllerBindings() {
968996
.leftTrigger()
969997
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
970998
.and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
971-
.whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this));
999+
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB));
1000+
soloController
1001+
.leftTrigger()
1002+
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
1003+
.and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
1004+
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB));
9721005
// Processor + Auto align right + Select scoring mode Coral
9731006
soloController
9741007
.rightTrigger()
@@ -977,7 +1010,8 @@ private void configureSoloControllerBindings() {
9771010
() -> {
9781011
Command scoreCommand =
9791012
switch (soloScoringMode) {
980-
case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand()
1013+
case CORAL_IN_CLAW -> getCoralBranchHeightCommand(
1014+
ScoringType.SOLOC_RIGHT)
9811015
.until(
9821016
() ->
9831017
soloController.a().getAsBoolean()
@@ -1005,7 +1039,12 @@ private void configureSoloControllerBindings() {
10051039
.rightTrigger()
10061040
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
10071041
.and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
1008-
.whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this));
1042+
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB));
1043+
soloController
1044+
.rightTrigger()
1045+
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
1046+
.and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
1047+
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB));
10091048
// Ground Intake
10101049
soloController
10111050
.leftBumper()
@@ -1095,11 +1134,5 @@ private void configureSoloControllerBindings() {
10951134
.startMovingVoltage(
10961135
() -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY()))
10971136
.withName("Elevator Manual Control"));
1098-
// Ready to score rumble
1099-
Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace());
1100-
readyToScore.onTrue(
1101-
Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0)));
1102-
readyToScore.onFalse(
1103-
Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0)));
11041137
}
11051138
}

‎src/main/java/frc/robot/generated/CompTunerConstants.java‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ public class CompTunerConstants {
5454
// When using closed-loop control, the drive motor uses the control
5555
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
5656
private static final Slot0Configs driveGains =
57-
new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
57+
new Slot0Configs().withKP(0.2).withKI(0).withKD(0).withKS(0.1).withKV(0.124).withKA(0);
5858

5959
// The closed-loop output type to use for the steer motors;
6060
// This affects the PID/FF gains for the steer motors
@@ -80,7 +80,14 @@ public class CompTunerConstants {
8080

8181
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
8282
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
83-
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
83+
private static final TalonFXConfiguration driveInitialConfigs =
84+
new TalonFXConfiguration()
85+
.withCurrentLimits(
86+
new CurrentLimitsConfigs()
87+
.withStatorCurrentLimit(Amps.of(160))
88+
.withStatorCurrentLimitEnable(true)
89+
.withSupplyCurrentLimit(Amps.of(100))
90+
.withSupplyCurrentLimitEnable(true));
8491
private static final TalonFXConfiguration steerInitialConfigs =
8592
new TalonFXConfiguration()
8693
.withCurrentLimits(
@@ -89,7 +96,9 @@ public class CompTunerConstants {
8996
// low
9097
// stator current limit to help avoid brownouts without impacting performance.
9198
.withStatorCurrentLimit(Amps.of(60))
92-
.withStatorCurrentLimitEnable(true));
99+
.withStatorCurrentLimitEnable(true)
100+
.withSupplyCurrentLimit(Amps.of(40))
101+
.withSupplyCurrentLimitEnable(true));
93102
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
94103
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
95104
private static final Pigeon2Configuration pigeonConfigs = null;

‎src/main/java/frc/robot/subsystems/SuperStructure.java‎

Lines changed: 94 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,19 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score)
6767
}
6868
}
6969

70+
private Command repeatPrescoreScoreSwing(
71+
Command command, BooleanSupplier score, BooleanSupplier ipScore) {
72+
// repeats scoring sequence if the coral is still in the claw
73+
if (armSensor == null) {
74+
return Commands.sequence(
75+
command,
76+
Commands.waitUntil(() -> !score.getAsBoolean()),
77+
Commands.waitUntil(ipScore).until(score));
78+
} else {
79+
return command.repeatedly().onlyWhile(armSensor.inClaw());
80+
}
81+
}
82+
7083
public Command coralLevelFour(BooleanSupplier score) {
7184
if (branchSensors != null) { // checks if sensor enabled then use for faster scoring
7285
score = branchSensors.withinScoreRange().or(score);
@@ -168,6 +181,85 @@ public Command coralLevelOne(BooleanSupplier score) {
168181
.withName("Coral Level 1");
169182
}
170183

184+
// New versions with ipScore
185+
// if robot is in position for intermediate scoring, it can score early but if vision is dead
186+
// manual control still works
187+
// only used on solo controls
188+
189+
public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4
190+
return Commands.sequence(
191+
Commands.parallel(
192+
elevator
193+
.setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS)
194+
.deadlineFor(
195+
armPivot
196+
.moveToPosition(ArmPivot.CORAL_PRESET_UP)
197+
.until(ipScore)
198+
.until(score)),
199+
spinnyClaw.stop())
200+
.withTimeout(0.5),
201+
repeatPrescoreScoreSwing(
202+
Commands.repeatingSequence(
203+
armPivot
204+
.moveToPosition(ArmPivot.CORAL_PRESET_L3)
205+
.withDeadline(Commands.waitUntil(ipScore).until(score)),
206+
armPivot
207+
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
208+
.withTimeout(1.5)
209+
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
210+
score,
211+
ipScore),
212+
coralPreIntake())
213+
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy())
214+
.withName("Coral Level 3");
215+
}
216+
217+
public Command coralLevelTwo(
218+
BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3
219+
return Commands.sequence(
220+
Commands.parallel(
221+
elevator
222+
.setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS)
223+
.deadlineFor(
224+
armPivot
225+
.moveToPosition(ArmPivot.CORAL_PRESET_UP)
226+
.until(ipScore)
227+
.until(score)),
228+
spinnyClaw.stop())
229+
.withTimeout(0.5),
230+
repeatPrescoreScoreSwing(
231+
Commands.sequence(
232+
armPivot
233+
.moveToPosition(ArmPivot.CORAL_PRESET_L2)
234+
.withDeadline(Commands.waitUntil(ipScore).until(score)),
235+
armPivot
236+
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
237+
.withTimeout(1.5)
238+
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
239+
score,
240+
ipScore),
241+
coralPreIntake())
242+
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy())
243+
.withName("Coral Level 2");
244+
}
245+
246+
public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) {
247+
return Commands.sequence(
248+
Commands.parallel(
249+
elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS),
250+
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1),
251+
spinnyClaw.stop()) // holds coral without wearing flywheels
252+
.withTimeout(0.5)
253+
.withDeadline(Commands.waitUntil(ipScore).until(score)),
254+
spinnyClaw
255+
.coralLevelOneHoldExtakePower()
256+
.withTimeout(0.5 /* this time could be shorter */), // spits out coral
257+
Commands.waitSeconds(1), // Wait to clear the reef
258+
coralPreIntake())
259+
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy())
260+
.withName("Coral Level 1");
261+
}
262+
171263
// quickGroundIntake() is used instead since it's faster and still reliable.
172264
// (This one moves the coral intake the normal intake position, then does the normal intake.
173265
// quickGroundIntake() instead hands the coral directly to the claw.)
@@ -216,11 +308,11 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph
216308
// Make the big sequence.
217309
return Commands.sequence(
218310
// Initial setup- Move elevator high enough for ground arm to be clear, start moving
219-
// arm pivot, stop the spinny claw, and start spinning the ground intake
311+
// arm pivot, and start spinning the ground intake
220312
Commands.parallel(
221313
elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE),
222314
armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
223-
spinnyClaw.stop(), // just as a backup in case things are silly
315+
spinnyClaw.coralIntakePower(),
224316
groundSpinny.setGroundIntakePower())
225317
// Move on even if arm isn't in position yet as long as elevator is high enough
226318
.until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)),

0 commit comments

Comments
 (0)