Skip to content

Commit 4c41176

Browse files
ds12ads12a
committed
l1 setpoints, stow at radius, sequential movements
1 parent 54bb29b commit 4c41176

File tree

9 files changed

+67
-44
lines changed

9 files changed

+67
-44
lines changed

‎src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,8 @@ public boolean isFinished() {
4949
if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) {
5050
return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
5151
|| robotStateSubsystem.getState() == RobotStates.LOADING_CORAL
52+
|| robotStateSubsystem.getState() == RobotStates.TO_ALGAE_CORAL_LOAD
53+
|| robotStateSubsystem.getState() == RobotStates.ALGAE_CORAL_LOAD
5254
|| !startingElevatorFinished;
5355
}
5456
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ public class AlgaeConstants {
3535
public static final double kSuperCycleHasAlgaeVelThres = 40;
3636
public static final int kHasAlgaeCounts = 2;
3737

38-
public static final double kHasCoralVelThreshold = 55; // FIXME
38+
public static final double kHasCoralVelThreshold = 70; // FIXME
3939
public static final int kHasCoralCounts = 10; // FIXME
4040

4141
public static final double kCoralScoringTime = 1; // FIXME

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ public class ElevatorConstants {
4949
public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint;
5050

5151
// Coral score
52-
public static final Angle kL1CoralSetpoint = Rotations.of(9.24); // 13.04053
52+
public static final Angle kL1CoralSetpoint = Rotations.of(3.931); // 13.04053
5353
public static final Angle kL2CoralSetpoint = Rotations.of(19.62793); // 19.62793 -> 21.0786 ->
5454
public static final Angle kL3CoralSetpoint =
5555
Rotations.of(31.0901); // was 30.42969 -> 31.7505 -> 31.0901

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -243,7 +243,7 @@ public static class ProtoConstants {
243243

244244
// Elevator
245245
public static Angle kElevatorFunnelSetpoint = Rotations.of(2.03125);
246-
public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.9);
246+
public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.76);
247247
public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint;
248248
public static Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5));
249249
public static Angle kMaxRev = kPrestageSetpoint.minus(Rotations.of(5));
@@ -349,7 +349,7 @@ public static TalonFXSConfiguration getFXSConfig() {
349349

350350
public static class CompConstants {
351351
public static Angle kElevatorFunnelSetpoint = Rotations.of(0.3676757);
352-
public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.9);
352+
public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.84);
353353
public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint;
354354

355355
// Biscuit
@@ -366,7 +366,7 @@ public static class CompConstants {
366366
// Idle
367367
public static Angle kBiscuitStowSetpoint = Rotations.of(1.862 / 2);
368368
public static Angle kFunnelSetpoint = kBiscuitStowSetpoint;
369-
public static Angle kL1CoralLoadSetpoint = Rotations.of(3.96);
369+
public static Angle kL1CoralLoadSetpoint = Rotations.of(8.078);
370370
public static Angle kPrestageSetpoint = Rotations.of(-2.94);
371371
public static Angle kPrestageAlgaeSetpoint = kBiscuitStowSetpoint;
372372

@@ -380,7 +380,7 @@ public static class CompConstants {
380380
public static double kTagAlignThreshold = 20.0 / 2;
381381

382382
// Coral score
383-
public static Angle kL1CoralSetpoint = Rotations.of(12.7);
383+
public static Angle kL1CoralSetpoint = Rotations.of(16.071);
384384
public static Angle kL2CoralSetpoint = kPrestageSetpoint;
385385
public static Angle kL3CoralSetpoint = kPrestageSetpoint;
386386
public static Angle kL4CoralSetpoint = kPrestageSetpoint;

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ public class RobotStateConstants {
1111
public static final double kAlgaeEjectTimer = 0.5;
1212

1313
public static final double kProcessorStowRadius = 0.5;
14+
public static final double kL1CoralStowRadius = 1.766;
1415

1516
public static final double kElevatorWaitRadius = 1.5;
1617

‎src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ public void updateInputs(AlgaeIOInputs inputs) {
5656
BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch);
5757
inputs.velocity = curVelocity.getValueAsDouble();
5858
inputs.isBeamBroken = fwdLimitSwitch.getValue().value == 0;
59-
inputs.isCoralBeamBroken = revLimitSwitch.getValue().value == 0;
59+
inputs.isCoralBeamBroken = revLimitSwitch.getValue().value == 1;
6060
}
6161

6262
@Override

‎src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ public void holdAlgae() {
6868

6969
public void holdCoral() {
7070
// setSpeed(AlgaeConstants.kCoralHoldSpeed); OR -AlgaeConstants.kHoldSpeed
71-
setPct(-0.2);
71+
setPct(-0.05);
7272
}
7373

7474
public boolean hasAlgae() {
@@ -118,24 +118,24 @@ public void periodic() {
118118
}
119119
}
120120
case HAS_CORAL -> {
121-
// if (!inputs.isCoralBeamBroken) {
122-
// setState(AlgaeStates.EMPTY);
123-
// }
121+
if (!inputs.isCoralBeamBroken) {
122+
setState(AlgaeStates.EMPTY);
123+
}
124124
}
125125
case CORAL_INTAKE -> {
126-
// if (inputs.isCoralBeamBroken) {
127-
if (FastMath.abs(inputs.velocity) < AlgaeConstants.kHasCoralVelThreshold) {
128-
slowCounts++;
129-
} else {
130-
slowCounts = 0;
131-
}
126+
if (inputs.isCoralBeamBroken) {
127+
if (FastMath.abs(inputs.velocity) < AlgaeConstants.kHasCoralVelThreshold) {
128+
slowCounts++;
129+
} else {
130+
slowCounts = 0;
131+
}
132132

133-
if (slowCounts >= AlgaeConstants.kHasCoralCounts) {
134-
slowCounts = 0;
135-
holdCoral();
136-
setState(AlgaeStates.HAS_CORAL);
133+
if (slowCounts >= AlgaeConstants.kHasCoralCounts) {
134+
slowCounts = 0;
135+
holdCoral();
136+
setState(AlgaeStates.HAS_CORAL);
137+
}
137138
}
138-
// }
139139
}
140140
case EMPTY -> {
141141
if (inputs.isBeamBroken) {

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import frc.robot.constants.BiscuitConstants;
1212
import frc.robot.constants.RobotConstants;
1313
import java.util.Set;
14+
import net.jafama.FastMath;
1415
import org.littletonrobotics.junction.Logger;
1516
import org.slf4j.LoggerFactory;
1617
import org.strykeforce.telemetry.TelemetryService;
@@ -97,9 +98,9 @@ public void periodic() {
9798

9899
switch (curState) {
99100
case NORMAL:
100-
// if (isFinished()
101-
// && FastMath.abs(inputs.velocity) <= BiscuitConstants.kZeroVelThresh
102-
// && setPoint != prevSetPoint) curState = BiscuitState.CHECK_ZERO;
101+
if (isFinished()
102+
&& FastMath.abs(inputs.velocity) <= BiscuitConstants.kZeroVelThresh
103+
&& setPoint != prevSetPoint) curState = BiscuitState.CHECK_ZERO;
103104
break;
104105
case CHECK_ZERO:
105106
prevSetPoint = setPoint;

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

Lines changed: 38 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,9 @@
11
package frc.robot.subsystems.robotState;
22

3-
import java.util.Set;
4-
5-
import org.littletonrobotics.junction.Logger;
6-
import org.slf4j.LoggerFactory;
7-
import org.strykeforce.telemetry.TelemetryService;
8-
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
9-
import org.strykeforce.telemetry.measurable.Measure;
10-
11-
import edu.wpi.first.math.geometry.Pose2d;
123
import static edu.wpi.first.units.Units.Rotations;
134
import static edu.wpi.first.units.Units.RotationsPerSecond;
5+
6+
import edu.wpi.first.math.geometry.Pose2d;
147
import edu.wpi.first.units.measure.Angle;
158
import edu.wpi.first.wpilibj.DriverStation.Alliance;
169
import edu.wpi.first.wpilibj.Timer;
@@ -37,7 +30,13 @@
3730
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
3831
import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates;
3932
import frc.robot.subsystems.vision.VisionSubsystem;
33+
import java.util.Set;
4034
import net.jafama.FastMath;
35+
import org.littletonrobotics.junction.Logger;
36+
import org.slf4j.LoggerFactory;
37+
import org.strykeforce.telemetry.TelemetryService;
38+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
39+
import org.strykeforce.telemetry.measurable.Measure;
4140

4241
public class RobotStateSubsystem extends MeasurableSubsystem {
4342
private org.slf4j.Logger logger = LoggerFactory.getLogger(this.getClass());
@@ -174,7 +173,7 @@ public boolean hasAlgae() {
174173
}
175174

176175
public boolean safeMoveElevator() {
177-
return !(coralSubsystem.getState() != CoralState.HAS_CORAL && funnelSubsystem.hasCoral() || algaeSubsystem.hasCoral());
176+
return !(coralSubsystem.getState() != CoralState.HAS_CORAL && funnelSubsystem.hasCoral());
178177
}
179178

180179
public void startupSequence() {
@@ -361,13 +360,12 @@ public void toAutonPrestage() {
361360
public void toFunnelLoad() {
362361

363362
if (scoringLevel == ScoringLevel.L1) {
364-
setBiscuitTransferSlow(RobotConstants.kL1CoralLoadSetpoint, true);
365363
elevatorSubsystem.setPosition(RobotConstants.kElevatorL1LoadSetpoint);
366364
algaeSubsystem.intakeCoral();
367365
funnelSubsystem.reverse();
368366
coralSubsystem.stop();
369367

370-
setState(RobotStates.ALGAE_CORAL_LOAD, true);
368+
setState(RobotStates.TO_ALGAE_CORAL_LOAD, false);
371369
} else {
372370
setBiscuitTransfer(RobotConstants.kFunnelSetpoint, true);
373371
elevatorSubsystem.setPosition(RobotConstants.kElevatorFunnelSetpoint);
@@ -429,9 +427,7 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
429427
if (drive) {
430428
tagAlignSubsystem.start(
431429
allianceColor,
432-
scoreSide == ScoreSide.LEFT
433-
|| !hasCoral()
434-
|| (wantAlgae && getAlgaeOnCycle),
430+
scoreSide == ScoreSide.LEFT || !hasCoral() || (wantAlgae && getAlgaeOnCycle),
435431
wantAlgae);
436432
setAutoPlacingLed(true);
437433
setState(RobotStates.REEF_ALIGN);
@@ -863,17 +859,27 @@ public void periodic() {
863859

864860
case PLACE_CORAL -> {
865861
if (!hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
862+
866863
coralLoc = CoralLoc.NONE;
867864
visionSubsystem.setYawUpdateCamera(-1);
868865
setAutoPlacingLed(false);
869866
driveSubsystem.setIgnoreSticks(false);
870-
toFunnelLoad();
867+
868+
if (scoringLevel == ScoringLevel.L1
869+
&& tagAlignSubsystem.getCurRadius(allianceColor)
870+
< RobotStateConstants.kL1CoralStowRadius) {
871+
break;
872+
}
873+
toStowSafe();
871874
} else if (hasCoral()) {
872875
coralLoc = CoralLoc.SCORING;
873876
}
874877
}
875878

876879
case FUNNEL_LOAD -> {
880+
if (scoringLevel == ScoringLevel.L1) {
881+
toFunnelLoad();
882+
}
877883
if (funnelSubsystem.hasCoral()) {
878884
coralLoc = CoralLoc.FUNNEL;
879885
}
@@ -885,8 +891,20 @@ public void periodic() {
885891
setState(RobotStates.LOADING_CORAL);
886892
}
887893
}
894+
case TO_ALGAE_CORAL_LOAD -> {
895+
if (scoringLevel != ScoringLevel.L1) {
896+
toFunnelLoad();
897+
}
898+
if (elevatorSubsystem.isFinished()) {
899+
setBiscuitTransfer(RobotConstants.kL1CoralLoadSetpoint, true);
900+
setState(RobotStates.ALGAE_CORAL_LOAD, true);
901+
}
902+
}
888903

889904
case ALGAE_CORAL_LOAD -> {
905+
if (scoringLevel != ScoringLevel.L1) {
906+
toFunnelLoad();
907+
}
890908
if (algaeSubsystem.hasCoral()) {
891909
coralLoc = CoralLoc.ALGAE;
892910
toPrestage();
@@ -927,9 +945,9 @@ public void periodic() {
927945
&& scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer)) {
928946
Pose2d currentPose = driveSubsystem.getPoseMeters();
929947
double distanceFromRelease =
930-
FastMath.hypot(
931-
currentPose.getX() - processorReleasePose.getX(),
932-
currentPose.getY() - processorReleasePose.getY());
948+
FastMath.sqrt(
949+
FastMath.pow(currentPose.getX() - processorReleasePose.getX(), 2)
950+
+ FastMath.pow(currentPose.getY() - processorReleasePose.getY(), 2));
933951
Logger.recordOutput("RobotState/Processor Release Distance", distanceFromRelease);
934952

935953
if (distanceFromRelease > RobotStateConstants.kProcessorStowRadius) {
@@ -1020,6 +1038,7 @@ public enum RobotStates {
10201038
REMOVE_ALGAE,
10211039
PLACE_CORAL,
10221040
FUNNEL_LOAD,
1041+
TO_ALGAE_CORAL_LOAD,
10231042
ALGAE_CORAL_LOAD,
10241043
LOADING_CORAL,
10251044
PRESTAGE,

0 commit comments

Comments
 (0)