Skip to content

Commit 5756f34

Browse files
caparomulacaparomula
authored
Vapor - Day One (#112)
* Reduce threshold for roller motion trigger, fix pose-seek logic * Fix typo and logic error * Tweaks * Day 1 changes * Spotless
1 parent a83e12c commit 5756f34

File tree

10 files changed

+55
-46
lines changed

10 files changed

+55
-46
lines changed

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

Lines changed: 12 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import static edu.wpi.first.units.Units.Centimeters;
44
import static edu.wpi.first.units.Units.Seconds;
55

6+
import edu.wpi.first.math.MathUtil;
67
import edu.wpi.first.math.geometry.Pose2d;
78
import edu.wpi.first.wpilibj.AddressableLED;
89
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
@@ -319,22 +320,20 @@ public void displayDefaultInfo(boolean isAlgaeMode, Optional<Gamepiece> gamepiec
319320
*/
320321
public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) {
321322
var delta = targetPose.minus(currentPose);
322-
var theta = delta.getRotation().getDegrees();
323-
fill(theta == 0 ? Color.kWhite : theta <= 180 ? Color.kMagenta : Color.kCyan, Segments.MIDDLE);
324-
325-
var x = delta.getTranslation().getX();
323+
var theta = MathUtil.inputModulus(delta.getRotation().getDegrees(), -180, 180);
326324
fill(
327-
(Centimeters.of(x)).baseUnitMagnitude() < 1
328-
? Color.kWhite
329-
: x > 0 ? Color.kGreen : Color.kRed,
330-
Segments.TOP);
325+
Math.abs(theta) < 3 ? Color.kWhite : theta > 0 ? Color.kMagenta : Color.kCyan,
326+
Segments.MIDDLE);
327+
328+
var x = delta.getTranslation().getMeasureX().in(Centimeters);
329+
fill(Math.abs(x) < 3 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
331330

332-
var y = delta.getTranslation().getY();
333-
if (Centimeters.of(y).baseUnitMagnitude() < 1) {
331+
var y = delta.getTranslation().getMeasureY().in(Centimeters);
332+
if (Math.abs(y) < 3) {
334333
fill(Color.kWhite, Segments.BOTTOM);
335334
} else {
336-
fill(Color.kGreen, y > 0 ? Segments.rightBottom : Segments.leftBottom);
337-
fill(Color.kBlack, y > 0 ? Segments.leftBottom : Segments.rightBottom);
335+
fill(Color.kGreen, y > 0 ? Segments.leftBottom : Segments.rightBottom);
336+
fill(Color.kBlack, y > 0 ? Segments.rightBottom : Segments.leftBottom);
338337
}
339338
}
340339

@@ -547,7 +546,7 @@ public Command createRollerAnimationCommand(
547546
@Override
548547
public void initialize() {
549548
var blocks = stackedBlocksPattern(colorSupplier.get(), 3, 2);
550-
scrollingBlocks = blocks.scrollAtRelativeSpeed(Seconds.of(1).asFrequency());
549+
scrollingBlocks = blocks.scrollAtRelativeSpeed(Seconds.of(0.5).asFrequency());
551550
buffers = isIntakeSupplier.getAsBoolean() ? intakeBuffers : outtakeBuffers;
552551
setName(
553552
String.format(

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

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,8 @@
4444
import frc.robot.elevator.AlgaeRoller;
4545
import frc.robot.elevator.CoralRoller;
4646
import frc.robot.elevator.Elevator;
47+
import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
48+
import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
4749
import frc.robot.elevator.Lifter;
4850
import frc.robot.vision.Vision;
4951
import frc.util.Gamepiece;
@@ -169,6 +171,8 @@ public void disabledPeriodic() {
169171
public void autonomousInit() {
170172
autoSelector.scheduleAuto();
171173
climber.lockRatchet();
174+
elevator.getCoralWrist().createSetAngleCommand(CoralWristState.Initial).schedule();
175+
elevator.getAlgaeWrist().createSetAngleCommand(AlgaeWristState.Initial).schedule();
172176
lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand());
173177
leds.replaceDefaultCommandImmediately(
174178
leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
@@ -187,7 +191,6 @@ public void teleopInit() {
187191
elevator.resetPositionControllers();
188192
lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
189193
// lifter.setDefaultCommand(lifter.createJoystickVoltageCommand(operator.getHID()));
190-
191194
leds.replaceDefaultCommandImmediately(
192195
leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
193196

@@ -320,6 +323,10 @@ public boolean getAsBoolean() {
320323

321324
// Auto climbe to position
322325
operator.povUp().onTrue(climber.createRetractCommand());
326+
327+
// just for testing roller animation.
328+
operator.povLeft().whileTrue(leds.createRollerAnimationCommand(() -> true, () -> Color.kOrange));
329+
operator.povRight().whileTrue(leds.createRollerAnimationCommand(() -> false, () -> Color.kOrange));
323330
}
324331

325332
private void configureEventBindings() {
@@ -329,7 +336,7 @@ private void configureEventBindings() {
329336
// .withTimeout(3))
330337
// ;
331338

332-
coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand());
339+
coralRoller.isRolling.whileTrue(createRollerAnimationCommand());
333340
// lifter.tooHighForCoralWrist.and(coralWrist.atRiskOfDamage)
334341
// .onTrue(coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode));
335342
}
@@ -393,6 +400,9 @@ protected void checkVision() {
393400
* @return An LED subsystem command that animates the rollers.
394401
*/
395402
protected Command createRollerAnimationCommand() {
403+
System.err.printf(
404+
"createRollerAnimation: algae=%b, coral=%b%n",
405+
algaeRoller.isRolling, coralRoller.isRolling);
396406
/*
397407
* On intake, one and only one is rolling
398408
*/

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

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -56,11 +56,8 @@ public AutoRoutine getAutoRoutine() {
5656
Commands.sequence(
5757
Commands.waitSeconds(0.1),
5858
coralRoller.createOuttakeCommand().withTimeout(0.2),
59-
Commands.waitSeconds(0.2),
60-
blueL4GBack.cmd()));
59+
Commands.waitSeconds(0.2)));
6160

62-
blueL4GBack.done().onTrue(
63-
elevator.coralIntakeCG());
6461
// spotless:on
6562

6663
return blueL4AutoRoutine;

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

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -53,11 +53,8 @@ public AutoRoutine getAutoRoutine() {
5353
elevator.coralL4PositionCG(),
5454
Commands.waitSeconds(0.1),
5555
coralRoller.createOuttakeCommand().withTimeout(0.2),
56-
Commands.waitSeconds(0.2),
57-
redL4GBack.cmd()));
56+
Commands.waitSeconds(0.2)));
5857

59-
redL4GBack.done().onTrue(
60-
elevator.coralIntakeCG());
6158
// spotless:on
6259

6360
return redL4AutoRoutine;

‎src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,12 @@ public ZorroDriveCommand(
1717

1818
@Override
1919
public double getX() {
20-
return -MathUtil.applyDeadband(m_controller.getRightYAxis(), 0.05);
20+
return -MathUtil.applyDeadband(m_controller.getRightYAxis(), 0.03);
2121
}
2222

2323
@Override
2424
public double getY() {
25-
return -MathUtil.applyDeadband(m_controller.getRightXAxis(), 0.05);
25+
return -MathUtil.applyDeadband(m_controller.getRightXAxis(), 0.03);
2626
}
2727

2828
@Override

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public class AlgaeRoller extends SubsystemBase {
2929
private final RelativeEncoder encoder = leaderMotor.getEncoder();
3030
private final SparkLimitSwitch algaeSensor = leaderMotor.getForwardLimitSwitch();
3131
public Trigger hasAlgae = new Trigger(() -> algaeSensor.isPressed());
32-
public final Trigger isRolling = new Trigger(() -> Math.abs(encoder.getVelocity()) > 80);
32+
public final Trigger isRolling = new Trigger(() -> Math.abs(encoder.getVelocity()) > 1);
3333

3434
public AlgaeRoller() {
3535
// spotless:off
@@ -69,7 +69,7 @@ public void periodic() {
6969
// SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
7070
// SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
7171
SmartDashboard.putBoolean("Algae Loaded", hasAlgae.getAsBoolean());
72-
SmartDashboard.putBoolean("Coral isRolling", isRolling.getAsBoolean());
72+
SmartDashboard.putBoolean("Algae isRolling", isRolling.getAsBoolean());
7373
}
7474

7575
private void setVoltage(Voltage voltage) {

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ public class AlgaeWrist extends SubsystemBase {
4141
private final ArmFeedforward feedforward =
4242
new ArmFeedforward(AlgaeWristConstants.kS, AlgaeWristConstants.kG, AlgaeWristConstants.kV);
4343

44-
private AlgaeWristState targetState = AlgaeWristState.Unknown;
44+
private AlgaeWristState targetState = AlgaeWristState.Initial;
4545

4646
public AlgaeWrist() {
4747
// spotless:off

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ public class CoralRoller extends SubsystemBase {
2424
private final RelativeEncoder encoder = motor.getEncoder();
2525
private final SparkLimitSwitch coralSensor = motor.getForwardLimitSwitch();
2626
public final Trigger hasCoral = new Trigger(() -> coralSensor.isPressed());
27-
public final Trigger isRolling = new Trigger(() -> Math.abs(encoder.getVelocity()) > 80);
27+
public final Trigger isRolling = new Trigger(() -> Math.abs(encoder.getVelocity()) > 1);
2828

2929
public CoralRoller() {
3030
// spotless:off

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ public class CoralWrist extends SubsystemBase {
4242
private final ArmFeedforward feedforward =
4343
new ArmFeedforward(CoralWristConstants.kS, CoralWristConstants.kG, CoralWristConstants.kV);
4444

45-
private CoralWristState targetState = CoralWristState.Unknown;
45+
private CoralWristState targetState = CoralWristState.Initial;
4646

4747
public CoralWrist() {
4848
// spotless:off

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

Lines changed: 22 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ public static final class LifterConstants {
3535
kPositionConversionFactor.per(Minutes);
3636

3737
public static final LinearVelocity kFineVelocity = InchesPerSecond.of(15.0);
38-
public static final LinearVelocity kRapidVelocity = InchesPerSecond.of(60.0);
38+
public static final LinearVelocity kRapidVelocity = InchesPerSecond.of(40.0);
3939
public static final LinearAccelerationUnit inchesPerSecondPerSecond =
4040
InchesPerSecond.per(Second);
4141
public static final LinearAcceleration kRapidAcceleration = inchesPerSecondPerSecond.of(1000);
@@ -64,7 +64,7 @@ public static enum LifterState {
6464
CoralL1(2.0),
6565
CoralL2(12.8),
6666
CoralL3(28.0),
67-
CoralL4(60.25),
67+
CoralL4(57.25),
6868
CoralIntake(11.7),
6969
AlgaeProcessor(12.0),
7070
AlgaeL2(28),
@@ -102,7 +102,7 @@ public static final class CoralWristConstants {
102102

103103
private static final double gearRatioMotortoEncoder = 1.0 / 5.0;
104104
private static final double gearRatioEncoderToArm = 24.0 / 42.0;
105-
private static final AngularVelocity maxMotorVelocity = RPM.of(11000);
105+
private static final AngularVelocity maxMotorVelocity = RPM.of(5000);
106106
private static final AngularVelocity maxArmVelocityTheoretical =
107107
maxMotorVelocity.times(gearRatioMotortoEncoder).times(gearRatioEncoderToArm);
108108
private static final AngularVelocity maxArmVelocityConstraint = DegreesPerSecond.of(90.0);
@@ -114,18 +114,19 @@ public static final class CoralWristConstants {
114114
maxArmAcceleration.in(RadiansPerSecondPerSecond));
115115

116116
/*
117-
When used as an absolute encoder, the CTRE SRX Mag encoder measures position
118-
in rotations at the sensor by default. Convert to radians at the algae wrist.
117+
* When used as an absolute encoder, the CTRE SRX Mag encoder measures position
118+
* in rotations at the sensor by default. Convert to radians at the algae wrist.
119119
*/
120120
public static final Angle kPositionConversionFactor = Rotations.of(gearRatioEncoderToArm);
121121
public static final Angle kZeroOffset =
122-
Radians.of(0.888).times(Rotations.of(1.0).div(kPositionConversionFactor));
122+
Degrees.of(63).times(Rotations.of(1.0).div(kPositionConversionFactor));
123123
public static final Angle kCenterOfGravityOffset = Degrees.of(0.0);
124124
public static final Angle kAllowableError = Degrees.of(3.0);
125125

126126
/*
127-
When used as an absolute encoder, the CTRE SRX Mag encoder measures velocity
128-
in rotations per minute at the sensor by default. Convert to radians per second at the algae wrist.
127+
* When used as an absolute encoder, the CTRE SRX Mag encoder measures velocity
128+
* in rotations per minute at the sensor by default. Convert to radians per
129+
* second at the algae wrist.
129130
*/
130131
public static final AngularVelocity kVelocityConversionFactor =
131132
Rotations.of(gearRatioEncoderToArm).per(Minute);
@@ -134,19 +135,20 @@ public static final class CoralWristConstants {
134135
public static final double kS = 0.15; // Found empirically 2/22/2025
135136
public static final double kV = (12.0 - kS) / maxArmVelocityTheoretical.in(RadiansPerSecond);
136137

137-
public static final double kP = 8.0;
138+
public static final double kP = 6.0;
138139
public static final double kI = 0.0;
139140
public static final double kD = 0.0;
140141
public static final Angle kIZone = Degrees.of(30.0);
141142

142143
public static enum CoralWristState {
144+
Initial(90),
143145
Unknown(90),
144146
Min(15),
145147
Max(135),
146148
L1(125),
147149
L2(65),
148150
L3(65),
149-
L4(28),
151+
L4(35),
150152
Intake(125),
151153
AlgaeMode(90);
152154

@@ -196,17 +198,20 @@ public static final class AlgaeWristConstants {
196198
maxArmAcceleration.in(RadiansPerSecondPerSecond));
197199

198200
/*
199-
When used as an absolute encoder, the REV Through Bore encoder measures position
200-
in rotations at the sensor by default. Convert to radians at the algae wrist.
201+
* When used as an absolute encoder, the REV Through Bore encoder measures
202+
* position
203+
* in rotations at the sensor by default. Convert to radians at the algae wrist.
201204
*/
202205
public static final Angle kPositionConversionFactor = Rotations.of(gearRatioEncoderToArm);
203206
public static final Angle kZeroOffset = Degrees.of(232.0);
204207
public static final Angle kCenterOfGravityOffset = Degrees.of(0.0);
205208
public static final Angle kAllowableError = Degrees.of(3.0);
206209

207210
/*
208-
When used as an absolute encoder, the REV Through Bore encoder measures velocity
209-
in rotations per minute at the sensor by default. Convert to radians per second at the algae wrist.
211+
* When used as an absolute encoder, the REV Through Bore encoder measures
212+
* velocity
213+
* in rotations per minute at the sensor by default. Convert to radians per
214+
* second at the algae wrist.
210215
*/
211216
public static final AngularVelocity kVelocityConversionFactor =
212217
Rotations.of(gearRatioEncoderToArm).per(Minute);
@@ -221,13 +226,14 @@ public static final class AlgaeWristConstants {
221226
public static final Angle kIZone = Degrees.of(30.0);
222227

223228
public static enum AlgaeWristState {
229+
Initial(80),
224230
Unknown(90),
225231
Floor(0),
226232
Min(-10),
227233
Max(95),
228234
Processor(0),
229-
L2(-0),
230-
L3(-0),
235+
L2(-5),
236+
L3(-5),
231237
Barge(60),
232238
CoralMode(80);
233239

0 commit comments

Comments
 (0)