Skip to content

Commit 5f46ae0

Browse files
nlaverdurenlaverdure
authored
Prevent auto drift (#141)
* publish profiledPIDcontrollers * wrist controllers never finish * refactor lifter controller and bindings * match height of lifter on teleop enable * change teleop bindings to ontrue * don't update state until command is called * ensure command names are reported * spotless * remove timeout from autoCG * remove parallel group in auto, bind commands from same trigger instead * move wait into next command sequence * remove unnecessary final wait * replace parallel CGs with instantCommands * reset position controllers in disabled periodic * whitespace * add names to algae wrist commands, reduce neo550 current limit * spotless
1 parent 8d4fea0 commit 5f46ae0

File tree

9 files changed

+192
-219
lines changed

9 files changed

+192
-219
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public static final class NEOConstants {
2929

3030
public static final class NEO550Constants {
3131
public static final AngularVelocity kFreeSpeed = RPM.of(11000);
32-
public static final int kDefaultCurrentLimit = 30;
32+
public static final int kDefaultCurrentLimit = 20;
3333
}
3434

3535
public static final class NEOVortexConstants {

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

Lines changed: 21 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,6 @@
4545
import frc.robot.drivetrain.commands.DriveToPoseCommand;
4646
import frc.robot.drivetrain.commands.ZorroDriveCommand;
4747
import frc.robot.elevator.AlgaeRoller;
48-
import frc.robot.elevator.AlgaeWrist;
4948
import frc.robot.elevator.CoralRoller;
5049
import frc.robot.elevator.Elevator;
5150
import frc.robot.elevator.Lifter;
@@ -66,8 +65,9 @@ public class Robot extends TimedRobot {
6665
private final Elevator elevator = new Elevator();
6766
private final Lifter lifter = elevator.getLifter();
6867
private final CoralRoller coralRoller = elevator.getCoralRoller();
68+
// private final CoralWrist coralWrist = elevator.getCoralWrist();
6969
private final AlgaeRoller algaeRoller = elevator.getAlgaeRoller();
70-
private final AlgaeWrist algaeWrist = elevator.getAlgaeWrist();
70+
// private final AlgaeWrist algaeWrist = elevator.getAlgaeWrist();
7171

7272
private final Drivetrain swerve =
7373
new Drivetrain(allianceSelector::fieldRotated, lifter::getProportionOfMaxHeight);
@@ -190,15 +190,16 @@ public void disabledPeriodic() {
190190

191191
allianceSelector.disabledPeriodic();
192192
autoSelector.disabledPeriodic();
193+
elevator.resetPositionControllers();
193194
}
194195

195196
@Override
196197
public void autonomousInit() {
197-
autoSelector.scheduleAuto();
198198
swerve.setDefaultCommand(swerve.createStopCommand());
199-
lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand());
199+
lifter.setDefaultCommand(lifter.remainAtCurrentHeight());
200200
leds.replaceDefaultCommandImmediately(
201201
leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
202+
autoSelector.scheduleAuto();
202203
}
203204

204205
@Override
@@ -209,11 +210,7 @@ public void teleopInit() {
209210
autoSelector.cancelAuto();
210211
swerve.setDefaultCommand(
211212
new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
212-
213-
lifter.matchHeight();
214-
lifter.resetController();
215-
lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
216-
// lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
213+
lifter.setDefaultCommand(lifter.joystickVelocityControl(operator.getHID()));
217214
leds.replaceDefaultCommandImmediately(
218215
leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
219216

@@ -316,26 +313,26 @@ public boolean getAsBoolean() {
316313
// operator.start().whileTrue(coralRoller.createOuttakeCommand());
317314

318315
// Configure to either score coral on L1 or score algae in processor
319-
operator.a().whileTrue(new ConditionalCommand(
316+
operator.a().onTrue(new ConditionalCommand(
320317
elevator.algaeProcessorPositionCG(), elevator.coralL1PositionCG(), algaeMode));
321318

322319
// Configure to either score coral on L2 or intake algae from L2
323-
operator.b().whileTrue(new ConditionalCommand(
320+
operator.b().onTrue(new ConditionalCommand(
324321
elevator.algaeL2IntakeCG(), elevator.coralL2PositionCG(), algaeMode));
325322

326323
// Configure to either score coral on L3 or intake algae from L3
327-
operator.x().whileTrue(new ConditionalCommand(
324+
operator.x().onTrue(new ConditionalCommand(
328325
elevator.algaeL3IntakeCG(), elevator.coralL3PositionCG(), algaeMode));
329326

330327
// Configure to either score coral on L4 or score algae in barge
331-
operator.y().whileTrue(new ConditionalCommand(
328+
operator.y().onTrue(new ConditionalCommand(
332329
elevator.algaeBargePositionCG(), elevator.coralL4PositionCG(), algaeMode));
333330

334331
// Configure to either intake coral from source or intake algae from floor
335332
// operator.start().whileTrue(new ConditionalCommand(
336333
// elevator.algaeFloorIntakeCG(), elevator.coralIntakeCG(), algaeMode));
337334

338-
operator.rightTrigger().whileTrue(new ConditionalCommand(
335+
operator.rightTrigger().onTrue(new ConditionalCommand(
339336
elevator.algaeFloorIntakeCG(), elevator.coralIntakeCG(), algaeMode));
340337

341338
// Intake coral and algae
@@ -345,8 +342,8 @@ public boolean getAsBoolean() {
345342

346343
// Force joystick operation of the elevator
347344
Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
348-
Trigger elevatorTriggerLow = operator.axisGreaterThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1);
349-
// elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
345+
Trigger elevatorTriggerLow = operator.axisLessThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1);
346+
elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.joystickVelocityControl(operator.getHID()));
350347

351348
// Actuate climber winch
352349
// Trigger climbTrigger = operator.axisGreaterThan(Axis.kRightY.value, -0.9, loop).debounce(0.1);
@@ -376,18 +373,15 @@ public boolean getAsBoolean() {
376373

377374
private void configureEventBindings() {
378375
RobotModeTriggers.autonomous()
379-
.onTrue(elevator.resetPositionControllers()
380-
.andThen(climber.lockRatchet()));
376+
.onTrue(climber.lockRatchet());
377+
381378
RobotModeTriggers.teleop()
382-
.onTrue(swerve.resetHeadingOffset()
383-
.andThen(elevator.resetPositionControllers())
384-
.andThen(climber.lockRatchet())
385-
.andThen(climber.resetEncoder()));
386-
387-
algaeRoller.hasAlgae
388-
.whileTrue(algaeRoller.createHoldAlgaeCommand());
389-
// algaeRoller.hasAlgae
390-
// .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
379+
.onTrue(swerve.resetHeadingOffset())
380+
.onTrue(lifter.matchHeight())
381+
.onTrue(climber.lockRatchet().andThen(climber.resetEncoder()));
382+
383+
algaeRoller.hasAlgae.onTrue(elevator.holdAlgaeCG());
384+
391385
coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand());
392386
}
393387

‎src/main/java/frc/robot/auto/BlueL4Auto.java

Lines changed: 5 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -45,16 +45,14 @@ public Optional<Pose2d> getInitialPose() {
4545
public AutoRoutine getAutoRoutine() {
4646

4747
// spotless:off
48-
blueL4AutoRoutine.active().onTrue(
49-
Commands.parallel(
50-
blueCenterToL4G.cmd(),
51-
elevator.coralL4PositionCG().withTimeout(2.0)));
48+
blueL4AutoRoutine.active()
49+
.onTrue(blueCenterToL4G.cmd())
50+
.onTrue(elevator.coralL4PositionCG());
5251

5352
blueCenterToL4G.done().onTrue(
5453
Commands.sequence(
55-
Commands.waitSeconds(0.1),
56-
coralRoller.createOuttakeCommand().withTimeout(0.2),
57-
Commands.waitSeconds(0.2)));
54+
Commands.waitSeconds(1.0),
55+
coralRoller.createOuttakeCommand().withTimeout(0.2)));
5856

5957
// spotless:on
6058

‎src/main/java/frc/robot/auto/RedL4Auto.java

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -45,17 +45,14 @@ public Optional<Pose2d> getInitialPose() {
4545
public AutoRoutine getAutoRoutine() {
4646

4747
// spotless:off
48-
redL4AutoRoutine.active().onTrue(
49-
Commands.parallel(
50-
redCenterToL4G.cmd(),
51-
elevator.coralL4PositionCG().withTimeout(2.0)));
52-
48+
redL4AutoRoutine.active()
49+
.onTrue(redCenterToL4G.cmd())
50+
.onTrue(elevator.coralL4PositionCG());
5351

5452
redCenterToL4G.done().onTrue(
5553
Commands.sequence(
56-
Commands.waitSeconds(0.1),
57-
coralRoller.createOuttakeCommand().withTimeout(0.2),
58-
Commands.waitSeconds(0.2)));
54+
Commands.waitSeconds(1.0),
55+
coralRoller.createOuttakeCommand().withTimeout(0.2)));
5956

6057
// spotless:on
6158

‎src/main/java/frc/robot/elevator/AlgaeRoller.java

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -82,22 +82,24 @@ public double getRollerVelocity() {
8282
}
8383

8484
public Command createStopCommand() {
85-
return this.startEnd(() -> leaderMotor.set(0.0), () -> {});
85+
return this.startEnd(() -> leaderMotor.set(0.0), () -> {}).withName("Stop");
8686
}
8787

8888
public Command createIntakeCommand() {
89-
return this.run(() -> setVoltage(AlgaeRollerConstants.kIntakeVoltage));
89+
return this.run(() -> setVoltage(AlgaeRollerConstants.kIntakeVoltage)).withName("Intake");
9090
}
9191

9292
public Command createOuttakeToBargeCommand() {
93-
return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToBargeVoltage));
93+
return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToBargeVoltage))
94+
.withName("Outtake to barge");
9495
}
9596

9697
public Command createOuttakeToProcessorCommand() {
97-
return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToProcessorVoltage));
98+
return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToProcessorVoltage))
99+
.withName("Outtake to processor");
98100
}
99101

100102
public Command createHoldAlgaeCommand() {
101-
return this.run(() -> setVoltage(AlgaeRollerConstants.kHoldVoltage));
103+
return this.run(() -> setVoltage(AlgaeRollerConstants.kHoldVoltage)).withName("Hold");
102104
}
103105
}

‎src/main/java/frc/robot/elevator/AlgaeWrist.java

Lines changed: 15 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,14 @@
2121
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2222
import edu.wpi.first.wpilibj2.command.Command;
2323
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
24+
import edu.wpi.first.wpilibj2.command.InstantCommand;
2425
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2526
import frc.robot.Constants.MotorConstants.NEOConstants;
2627
import frc.robot.Constants.RobotConstants;
2728
import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants;
2829
import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
2930
import java.util.function.BooleanSupplier;
31+
import java.util.function.Supplier;
3032

3133
public class AlgaeWrist extends SubsystemBase {
3234

@@ -84,7 +86,7 @@ public AlgaeWrist(BooleanSupplier hasAlgaeSupplier) {
8486
feedback.enableContinuousInput(0, 2.0 * Math.PI); // TODO: determine if has any effect
8587
// feedback.setIntegratorRange();
8688

87-
setDefaultCommand(createRemainAtCurrentAngleCommand());
89+
setDefaultCommand(remainAtCurrentAngle());
8890
}
8991

9092
@Override
@@ -101,6 +103,7 @@ public void periodic() {
101103
// SmartDashboard.putNumber("Algae Wrist/Applied Duty Cycle", motor.getAppliedOutput());
102104
// SmartDashboard.putNumber("Algae Wrist/Current", motor.getOutputCurrent());
103105
SmartDashboard.putBoolean("Algae Wrist/At Goal", feedback.atGoal());
106+
SmartDashboard.putData("Algae Wrist/Controller", feedback);
104107
}
105108

106109
private Angle getCurrentAngle() {
@@ -135,35 +138,20 @@ public AlgaeWristState getTargetState() {
135138
return this.targetState;
136139
}
137140

138-
public Command createSetAngleCommand(AlgaeWristState state) {
139-
return new FunctionalCommand(
140-
// initialize
141-
() -> {
142-
targetState = state;
143-
feedback.setGoal(targetState.angle.in(Radians));
144-
},
145-
// execute
146-
() -> control(),
147-
// end
148-
interrupted -> {},
149-
// isFinished
150-
() -> feedback.atGoal(),
151-
// requirements
152-
this);
141+
public Command remainAtCurrentAngle() {
142+
return setAngle(() -> getCurrentAngle()).withName("Maintain current angle");
153143
}
154144

155-
public Command createRemainAtCurrentAngleCommand() {
145+
public Command setAngle(AlgaeWristState state) {
146+
return setAngle(() -> state.angle)
147+
.beforeStarting(new InstantCommand(() -> targetState = state))
148+
.withName("Set angle to " + state.toString());
149+
}
150+
151+
public Command setAngle(Supplier<Angle> angleSupplier) {
156152
return new FunctionalCommand(
157153
// initialize
158-
() -> {
159-
if (targetState == AlgaeWristState.Initial) {
160-
targetState = AlgaeWristState.CoralMode;
161-
feedback.setGoal(targetState.angle.in(Radians));
162-
// Users should call reset() when they first start running the controller to avoid
163-
// unwanted behavior.
164-
resetController();
165-
}
166-
},
154+
() -> feedback.setGoal(angleSupplier.get().in(Radians)),
167155
// execute
168156
() -> control(),
169157
// end
@@ -174,7 +162,7 @@ public Command createRemainAtCurrentAngleCommand() {
174162
this);
175163
}
176164

177-
public Command createJoystickControlCommand(XboxController gamepad) {
165+
public Command joystickVoltageControl(XboxController gamepad) {
178166
return this.run(
179167
() -> {
180168
double joystickInput = MathUtil.applyDeadband(-gamepad.getLeftY(), 0.05);

‎src/main/java/frc/robot/elevator/CoralWrist.java

Lines changed: 14 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,13 @@
2121
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2222
import edu.wpi.first.wpilibj2.command.Command;
2323
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
24+
import edu.wpi.first.wpilibj2.command.InstantCommand;
2425
import edu.wpi.first.wpilibj2.command.SubsystemBase;
25-
import edu.wpi.first.wpilibj2.command.button.Trigger;
2626
import frc.robot.Constants.MotorConstants.NEO550Constants;
2727
import frc.robot.Constants.RobotConstants;
2828
import frc.robot.elevator.ElevatorConstants.CoralWristConstants;
2929
import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
30+
import java.util.function.Supplier;
3031

3132
public class CoralWrist extends SubsystemBase {
3233

@@ -83,7 +84,7 @@ public CoralWrist() {
8384
feedback.enableContinuousInput(0, 2.0 * Math.PI); // TODO: determine if has any effect
8485
// controller.setIntegratorRange();
8586

86-
setDefaultCommand(createRemainAtCurrentAngleCommand());
87+
setDefaultCommand(remainAtCurrentAngle());
8788
}
8889

8990
@Override
@@ -100,6 +101,7 @@ public void periodic() {
100101
// SmartDashboard.putNumber("Coral Wrist/Applied Duty Cycle", motor.getAppliedOutput());
101102
// SmartDashboard.putNumber("Coral Wrist/Current", motor.getOutputCurrent());
102103
SmartDashboard.putBoolean("Coral Wrist/At Goal", feedback.atGoal());
104+
SmartDashboard.putData("Coral Wrist/Controller", feedback);
103105
}
104106

105107
private Angle getCurrentAngle() {
@@ -132,38 +134,20 @@ public Boolean inState(CoralWristState state) {
132134
return this.targetState.equals(state);
133135
}
134136

135-
public Trigger atRiskOfDamage =
136-
new Trigger(
137-
() -> getCurrentAngle().gt(CoralWristState.AlgaeMode.angle.plus(Degrees.of(3.0))));
137+
public Command remainAtCurrentAngle() {
138+
return setAngle(() -> getCurrentAngle()).withName("Maintain current angle");
139+
}
138140

139-
public Command createSetAngleCommand(CoralWristState state) {
140-
return new FunctionalCommand(
141-
// initialize
142-
() -> {
143-
targetState = state;
144-
feedback.setGoal(targetState.angle.in(Radians));
145-
},
146-
// execute
147-
() -> control(),
148-
// end
149-
interrupted -> {},
150-
// isFinished
151-
() -> feedback.atGoal(),
152-
// requirements
153-
this);
141+
public Command setAngle(CoralWristState state) {
142+
return setAngle(() -> state.angle)
143+
.beforeStarting(new InstantCommand(() -> targetState = state))
144+
.withName("Set angle to " + state.toString());
154145
}
155146

156-
public Command createRemainAtCurrentAngleCommand() {
147+
public Command setAngle(Supplier<Angle> angleSupplier) {
157148
return new FunctionalCommand(
158149
// initialize
159-
() -> {
160-
if (targetState == CoralWristState.Initial) {
161-
feedback.setGoal(encoder.getPosition());
162-
// Users should call reset() when they first start running the controller to avoid
163-
// unwanted behavior.
164-
resetController();
165-
}
166-
},
150+
() -> feedback.setGoal(angleSupplier.get().in(Radians)),
167151
// execute
168152
() -> control(),
169153
// end
@@ -174,7 +158,7 @@ public Command createRemainAtCurrentAngleCommand() {
174158
this);
175159
}
176160

177-
public Command createJoystickControlCommand(XboxController gamepad) {
161+
public Command joystickVoltageControl(XboxController gamepad) {
178162
return this.run(
179163
() -> {
180164
double joystickInput = 2.0 * MathUtil.applyDeadband(-gamepad.getLeftY(), 0.05);

0 commit comments

Comments
 (0)