Skip to content

Commit ea25f4b

Browse files
ds12ads12a
committed
score over sideways coral
2 parents 4c41176 + 70e8287 commit ea25f4b

File tree

12 files changed

+386
-253
lines changed

12 files changed

+386
-253
lines changed

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

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@
4949
import frc.robot.commands.robotState.HPAlgaeCommand;
5050
import frc.robot.commands.robotState.InterruptAutoCommand;
5151
import frc.robot.commands.robotState.LockWheelsCommand;
52+
import frc.robot.commands.robotState.OperatorRumbleCommand;
5253
import frc.robot.commands.robotState.ReefCycleCommand;
5354
import frc.robot.commands.robotState.ScoreAlgaeCommand;
5455
import frc.robot.commands.robotState.SetScoreSideCommand;
@@ -425,7 +426,9 @@ private void configureOperatorBindings() {
425426
new ClimbPrepCommand(
426427
robotStateSubsystem, climbSubsystem, elevatorSubsystem, biscuitSubsystem));
427428

428-
////////////////////////// TODO: REMOVE!!! /////////////////////////////
429+
new Trigger(() -> robotStateSubsystem.isStuckAndMisaligned())
430+
.onTrue(new OperatorRumbleCommand(robotStateSubsystem, xboxController));
431+
429432
// Move biscuit
430433
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
431434
.onTrue(
@@ -449,7 +452,6 @@ private void configureOperatorBindings() {
449452
new JogElevatorCommand(
450453
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
451454
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
452-
//////////////////////////////////////////////////////////////////
453455
}
454456

455457
private void configureTestOperatorBindings() {

‎src/main/java/frc/robot/commands/auton/DriveAutonServoCommand.java‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -174,8 +174,7 @@ public void execute() {
174174
hasStaged = true;
175175
robotStateSubsystem.toAutonPrestage();
176176
}
177-
if (tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor())
178-
<= AutonConstants.kElevatorStageRadiusPathOne
177+
if (tagAlignSubsystem.getCurRadius() <= AutonConstants.kElevatorStageRadiusPathOne
179178
&& !hasPreppedCoral) {
180179
hasPreppedCoral = true;
181180
robotStateSubsystem.toPrepCoral();
@@ -189,6 +188,7 @@ public void execute() {
189188
isServoing = true;
190189
tagAlignSubsystem.startAuto(
191190
mirrorTrajectory ? Alliance.Red : Alliance.Blue,
191+
robotStateSubsystem.getCoralLevel(),
192192
mirrorToProcessor ? !scoreLeft : scoreLeft,
193193
false);
194194
}
@@ -199,8 +199,7 @@ public void execute() {
199199
}
200200

201201
private boolean shouldTransitionToServoing() {
202-
return tagAlignSubsystem.getCurRadius(mirrorTrajectory ? Alliance.Red : Alliance.Blue)
203-
< PathHandlerConstants.kServoRadius;
202+
return tagAlignSubsystem.getCurRadius() < PathHandlerConstants.kServoRadius;
204203
}
205204

206205
@Override

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

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
4+
import edu.wpi.first.wpilibj.Timer;
5+
import edu.wpi.first.wpilibj.XboxController;
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
8+
9+
public class OperatorRumbleCommand extends Command {
10+
11+
private RobotStateSubsystem robotStateSubsystem;
12+
private Timer timer;
13+
private XboxController xboxController;
14+
private boolean hasStartedTimer = false;
15+
16+
public OperatorRumbleCommand(
17+
RobotStateSubsystem robotStateSubsystem, XboxController xboxController) {
18+
this.xboxController = xboxController;
19+
timer = new Timer();
20+
}
21+
22+
@Override
23+
public void initialize() {
24+
timer.reset();
25+
timer.start();
26+
}
27+
28+
@Override
29+
public void execute() {
30+
xboxController.setRumble(RumbleType.kBothRumble, 1);
31+
}
32+
33+
@Override
34+
public boolean isFinished() {
35+
return timer.hasElapsed(0.5);
36+
}
37+
38+
@Override
39+
public void end(boolean interrupted) {
40+
xboxController.setRumble(RumbleType.kBothRumble, 0);
41+
}
42+
43+
@Override
44+
public boolean runsWhenDisabled() {
45+
return true;
46+
}
47+
}

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,8 @@ public boolean isFinished() {
4444
return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD;
4545
} else {
4646
if (startingRobotState == RobotStates.PRESTAGE || startingRobotState == RobotStates.STOW) {
47-
return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL;
47+
return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL
48+
|| (!robotStateSubsystem.hasCoral() && !robotStateSubsystem.getGetAlgaeOnCycle());
4849
}
4950
if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) {
5051
return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD

‎src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java‎

Lines changed: 0 additions & 56 deletions
This file was deleted.

‎src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java‎

Lines changed: 0 additions & 26 deletions
This file was deleted.

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

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,9 @@
11
package frc.robot.constants;
22

3+
import static edu.wpi.first.units.Units.Rotations;
4+
5+
import edu.wpi.first.units.measure.Angle;
6+
37
public class RobotStateConstants {
48
public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
59
public static final double kAlgaeRetreatDistance = 0;
@@ -24,4 +28,84 @@ public class RobotStateConstants {
2428
// Climb LED Thresholds
2529
public static final double kClimbAngleSmall = -0.235; // -0.245
2630
public static final double kClimbAngleBig = -0.213; // -0.215
31+
32+
// Offsets
33+
public static final Angle kStuckCoralElevatorOffset = Rotations.of(3.746);
34+
public static final Angle[][][] kBlueCoralElevatorOffset = {
35+
{
36+
{Rotations.of(0), Rotations.of(0)},
37+
{Rotations.of(0), Rotations.of(0)},
38+
{Rotations.of(0), Rotations.of(0)},
39+
{Rotations.of(0), Rotations.of(0)}
40+
},
41+
{
42+
{Rotations.of(0), Rotations.of(0)},
43+
{Rotations.of(0), Rotations.of(0)},
44+
{Rotations.of(0), Rotations.of(0)},
45+
{Rotations.of(0), Rotations.of(0)}
46+
},
47+
{
48+
{Rotations.of(0), Rotations.of(0)},
49+
{Rotations.of(0), Rotations.of(0)},
50+
{Rotations.of(0), Rotations.of(0)},
51+
{Rotations.of(0), Rotations.of(0)}
52+
},
53+
{
54+
{Rotations.of(0), Rotations.of(0)},
55+
{Rotations.of(0), Rotations.of(0)},
56+
{Rotations.of(0), Rotations.of(0)},
57+
{Rotations.of(0), Rotations.of(0)}
58+
},
59+
{
60+
{Rotations.of(0), Rotations.of(0)},
61+
{Rotations.of(0), Rotations.of(0)},
62+
{Rotations.of(0), Rotations.of(0)},
63+
{Rotations.of(0), Rotations.of(0)}
64+
},
65+
{
66+
{Rotations.of(0), Rotations.of(0)},
67+
{Rotations.of(0), Rotations.of(0)},
68+
{Rotations.of(0), Rotations.of(0)},
69+
{Rotations.of(0), Rotations.of(0)}
70+
}
71+
};
72+
73+
public static final Angle[][][] kRedCoralElevatorOffset = {
74+
{
75+
{Rotations.of(0), Rotations.of(0)},
76+
{Rotations.of(0), Rotations.of(0)},
77+
{Rotations.of(0), Rotations.of(0)},
78+
{Rotations.of(0), Rotations.of(0)}
79+
},
80+
{
81+
{Rotations.of(0), Rotations.of(0)},
82+
{Rotations.of(0), Rotations.of(0)},
83+
{Rotations.of(0), Rotations.of(0)},
84+
{Rotations.of(0), Rotations.of(0)}
85+
},
86+
{
87+
{Rotations.of(0), Rotations.of(0)},
88+
{Rotations.of(0), Rotations.of(0)},
89+
{Rotations.of(0), Rotations.of(0)},
90+
{Rotations.of(0), Rotations.of(0)}
91+
},
92+
{
93+
{Rotations.of(0), Rotations.of(0)},
94+
{Rotations.of(0), Rotations.of(0)},
95+
{Rotations.of(0), Rotations.of(0)},
96+
{Rotations.of(0), Rotations.of(0)}
97+
},
98+
{
99+
{Rotations.of(0), Rotations.of(0)},
100+
{Rotations.of(0), Rotations.of(0)},
101+
{Rotations.of(0), Rotations.of(0)},
102+
{Rotations.of(0), Rotations.of(0)}
103+
},
104+
{
105+
{Rotations.of(0), Rotations.of(0)},
106+
{Rotations.of(0), Rotations.of(0)},
107+
{Rotations.of(0), Rotations.of(0)},
108+
{Rotations.of(0), Rotations.of(0)}
109+
}
110+
};
27111
}

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

Lines changed: 33 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,8 @@ public class TagServoingConstants {
2525

2626
public static final Constraints alignXConstraints = new Constraints(2, 100000);
2727
public static final Constraints alignYConstraints = new Constraints(2, 100000);
28-
// public static final Constraints alignOmegaConstraints = new Constraints(10000, 20000);
28+
// public static final Constraints alignOmegaConstraints = new
29+
// Constraints(10000, 20000);
2930

3031
public static final double[] kAngleTarget = {
3132
Units.degreesToRadians(0),
@@ -40,6 +41,24 @@ public class TagServoingConstants {
4041
public static final int[] kBlueTargetTag = {18, 17, 22, 21, 20, 19};
4142
public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6};
4243

44+
// Offsets
45+
public static final double[][][] kBlueCoralOffset = {
46+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
47+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
48+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
49+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
50+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
51+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}}
52+
}; // Alliance relative hexant, level, left/right
53+
public static final double[][][] kRedCoralOffset = {
54+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
55+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
56+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
57+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
58+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
59+
{{0, 0}, {0, 0}, {0, 0}, {0, 0}}
60+
}; // Alliance relative hexant, level, left/right
61+
4362
// Tag align
4463
public static final double kHorizontalCloseEnough = 20;
4564
public static final double kAngleCloseEnough = Units.degreesToRadians(1.0);
@@ -50,11 +69,16 @@ public class TagServoingConstants {
5069
public static final double kCoralInitialDriveRadius = 1.7; // 0.36 away from reef wall
5170
public static final double kCoralAlignRadius = 1.32; // 1.293823 is perfectly against the reef
5271
// public static final double kCoralStopXDriveRadius =
53-
// kCoralInitialDriveRadius; // Should be closer to reef than target pose
72+
// kCoralInitialDriveRadius; // Should be closer to reef than target pose
5473
public static final double kAlgaeInitialDriveRadius = kCoralInitialDriveRadius; // 1.75;
5574
public static final double kAlgaeAlignRadius = kCoralAlignRadius; // 1.34; // was 1.34
5675
public static final double kAlgaeStopXDriveRadius =
57-
kAlgaeInitialDriveRadius; // Should be closer to reef than target pose
76+
kAlgaeInitialDriveRadius; // Should be closer to reef than target
77+
public static final double kL1CoralRadius = 1.5;
78+
79+
// Final drive
80+
public static final double kFinalDriveVel = 0.25;
81+
// pose
5882
// public static final double kMinVelX = 0.85;
5983

6084
// End conditions
@@ -70,8 +94,13 @@ public class TagServoingConstants {
7094
public static final int kEndCountThreshold = 1;
7195
public static final double kEndVelThreshold = 5;
7296

97+
// Stuck coral
98+
public static final double kMaxStalledDer = 0.5;
99+
public static final double kMinStuckCounts = 5;
100+
public static final double kCoralStuckRadius = 1.4;
101+
public static final double kCoralStuckAllowence = 0.03;
102+
73103
// Reef
74104
public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259);
75-
76105
public static final Translation2d kRedReefPose = new Translation2d(13.058902, 4.0259);
77106
}

0 commit comments

Comments
 (0)